/*
 * (C) Copyright 2018-2019 UCAR
 *
 * This software is licensed under the terms of the Apache Licence Version 2.0
 * which can be obtained at http://www.apache.org/licenses/LICENSE-2.0.
 */

#include <Eigen/Core>
#include <cmath>
#include <fstream>
#include <memory>
#include <random>
#include <set>

#include "ufo/ObsBiasCovariance.h"

#include "ioda/distribution/Accumulator.h"
#include "ioda/Engines/Factory.h"
#include "ioda/Engines/HH.h"
#include "ioda/Layout.h"
#include "ioda/ObsGroup.h"
#include "ioda/ObsSpace.h"
#include "ioda/ObsVector.h"

#include "oops/util/IntSetParser.h"
#include "oops/util/Logger.h"
#include "oops/util/Random.h"

#include "ufo/ObsBias.h"
#include "ufo/ObsBiasIncrement.h"
#include "ufo/ObsBiasPreconditioner.h"
#include "ufo/predictors/PredictorBase.h"
#include "ufo/utils/IodaGroupIndices.h"

namespace ufo {

// -----------------------------------------------------------------------------

ObsBiasCovariance::ObsBiasCovariance(ioda::ObsSpace & odb,
                                     const Parameters_ & params)
  : odb_(odb), prednames_(0), vars_(odb.obsvariables()), variances_(),
    preconditioner_(0),
    ht_rinv_h_(0), obs_num_(0), analysis_variances_(0), minimal_required_obs_number_(0),
    rank_(odb.distribution()->rank()) {
  oops::Log::trace() << "ObsBiasCovariance::Constructor starting" << std::endl;

  // Predictor factory
  for (const PredictorParametersWrapper &wrapper :
       params.variationalBC.value().predictors.value()) {
    std::shared_ptr<PredictorBase> pred(PredictorFactory::create(wrapper.predictorParameters,
                                                                 vars_));
    prednames_.push_back(pred->name());
  }

  if (prednames_.size()*vars_.size() > 0) {
    if (params.covariance.value() == boost::none)
      throw eckit::UserError("obs bias.covariance section missing from the YAML file");
    const ObsBiasCovarianceParameters &biasCovParams = *params.covariance.value();

    // Get the minimal required filtered obs number
    minimal_required_obs_number_ = biasCovParams.minimalRequiredObsNumber;

    // Override the variance range if provided
    {
      const std::vector<double> &range = biasCovParams.varianceRange.value();
      ASSERT(range.size() == 2);
      smallest_variance_ = range[0];
      largest_variance_ = range[1];
    }

    // Override the preconditioning step size if provided
    step_size_ = biasCovParams.stepSize;

    // Override the largest analysis variance if provided
    largest_analysis_variance_ = biasCovParams.largestAnalysisVariance;

    // Initialize the variances to upper limit
    variances_ = Eigen::VectorXd::Constant(prednames_.size()*vars_.size(), largest_variance_);

    // Initialize the hessian contribution to zero
    ht_rinv_h_.resize(prednames_.size() * vars_.size());
    std::fill(ht_rinv_h_.begin(), ht_rinv_h_.end(), 0.0);

    // Initialize the preconditioner to default step size
    preconditioner_.resize(prednames_.size() * vars_.size());
    std::fill(preconditioner_.begin(), preconditioner_.end(), step_size_);

    // Initialize obs_num_ to ZERO
    obs_num_.resize(vars_.size());
    std::fill(obs_num_.begin(), obs_num_.end(), 0);

    // Initialize analysis error variances to the upper limit
    analysis_variances_.resize(prednames_.size() * vars_.size());
    std::fill(analysis_variances_.begin(), analysis_variances_.end(), largest_variance_);

    // Initializes from given prior
    if (biasCovParams.prior.value() != boost::none) {
      const ObsBiasCovariancePriorParameters &priorParams = *biasCovParams.prior.value();

      // Get default inflation ratio
      const double inflation_ratio = priorParams.inflation.value().ratio;

      // Check the large inflation ratio when obs number < minimal_required_obs_number
      const double large_inflation_ratio = priorParams.inflation.value().ratioForSmallDataset;

      // read in Variances prior (analysis_variances_) and number of obs. (obs_num_)
      // from previous cycle
      this->read(priorParams);

      // set variances for bias predictor coeff. based on diagonal info
      // of previous analysis error variance
      std::size_t ii;
      for (std::size_t j = 0; j < vars_.size(); ++j) {
        const double inflation = (obs_num_[j] <= minimal_required_obs_number_) ?
                                 large_inflation_ratio : inflation_ratio;
        for (std::size_t p = 0; p < prednames_.size(); ++p) {
          ii = j*prednames_.size() + p;
          if (inflation > inflation_ratio)
            analysis_variances_[ii] = inflation * analysis_variances_[ii] + smallest_variance_;
          variances_[ii] = inflation * analysis_variances_[ii] + smallest_variance_;
          if (variances_[ii] > largest_variance_) variances_[ii] = largest_variance_;
          if (analysis_variances_[ii] > largest_analysis_variance_)
            analysis_variances_[ii] = largest_analysis_variance_;
        }
      }
    }
  }

  oops::Log::trace() << "ObsBiasCovariance::Constructor is done" << std::endl;
}

// -----------------------------------------------------------------------------

void ObsBiasCovariance::read(const ObsBiasCovariancePriorParameters & params) {
  oops::Log::trace() << "ObsBiasCovariance::read from file " << std::endl;

  if (params.inputFile.value() != boost::none) {
    // Open an hdf5 file, read only
    ioda::Engines::BackendNames  backendName = ioda::Engines::BackendNames::Hdf5File;
    ioda::Engines::BackendCreationParameters backendParams;
    backendParams.fileName = *params.inputFile.value();
    backendParams.action   = ioda::Engines::BackendFileActions::Open;
    backendParams.openMode = ioda::Engines::BackendOpenModes::Read_Only;

    // Create the backend and attach it to an ObsGroup
    // Use the None DataLyoutPolicy for now to accommodate the current file format
    ioda::Group backend = constructBackend(backendName, backendParams);
    ioda::ObsGroup obsgroup = ioda::ObsGroup(backend,
                   ioda::detail::DataLayoutPolicy::generate(
                         ioda::detail::DataLayoutPolicy::Policies::None));

    // Read coefficients error variances into the Eigen array
    ioda::Variable bcerrvar = obsgroup.vars["bias_coeff_errors"];
    Eigen::ArrayXXf allbcerrors;
    bcerrvar.readWithEigenRegular(allbcerrors);

    // Read nobs into Eigen array
    ioda::Variable nobsvar = obsgroup.vars["number_obs_assimilated"];
    Eigen::ArrayXf nobsassim;
    nobsvar.readWithEigenRegular(nobsassim);

    // Find indices of predictors and variables/channels that we need in the data read from the file
    const std::vector<int> pred_idx = getRequiredVariableIndices(obsgroup, "predictors",
                                              prednames_.begin(), prednames_.end());
    const std::vector<int> var_idx = getRequiredVarOrChannelIndices(obsgroup, vars_);

    // Filter predictors and channels that we need
    // FIXME: may be possible by indexing allbcerrors(pred_idx, chan_idx) when Eigen 3.4
    // is available
    for (size_t jvar = 0; jvar < var_idx.size(); ++jvar) {
      obs_num_[jvar] = nobsassim(var_idx[jvar]);
      for (size_t jpred = 0; jpred < pred_idx.size(); ++jpred) {
        analysis_variances_[jvar*pred_idx.size()+jpred] =
             allbcerrors(pred_idx[jpred], var_idx[jvar]);
      }
    }
  }
  oops::Log::trace() << "ObsBiasCovariance::read is done " << std::endl;
}

// -----------------------------------------------------------------------------

void ObsBiasCovariance::write(const Parameters_ & params) {
  // only write files out on the task with MPI rank 0
  if (rank_ != 0) return;

  oops::Log::trace() << "ObsBiasCovariance::write to file " << std::endl;
  const ObsBiasCovarianceParameters &biasCovParams = *params.covariance.value();

  if (biasCovParams.outputFile.value() != boost::none) {
    // FIXME: only implemented for channels currently
    if (vars_.channels().size() == 0) {
      throw eckit::NotImplemented("ObsBiasCovariance::write not implemented for without channels",
                                  Here());
    }
    // Create a file, overwrite if exists
    const std::string output_filename = *biasCovParams.outputFile.value();
    ioda::Group group = ioda::Engines::HH::createFile(output_filename,
                        ioda::Engines::BackendCreateModes::Truncate_If_Exists);

    // put only variable bias predictors into the predictors vector
    std::vector<std::string> predictors(prednames_.begin(), prednames_.end());
    // map coefficients to 2D for saving
    Eigen::Map<const Eigen::MatrixXd>
        allbcerrors(variances_.data(), prednames_.size(), vars_.size());
    const std::vector<int> channels = vars_.channels();
    std::vector<int> obs_assimilated(obs_num_.begin(), obs_num_.end());

    // dimensions
    ioda::NewDimensionScales_t dims {
        ioda::NewDimensionScale<int>("npredictors", predictors.size()),
        ioda::NewDimensionScale<int>("nchannels", channels.size())
    };
    // new ObsGroup
    ioda::ObsGroup ogrp = ioda::ObsGroup::generate(group, dims);

    // save the predictors
    ioda::Variable predsVar = ogrp.vars.createWithScales<std::string>(
                              "predictors", {ogrp.vars["npredictors"]});
    predsVar.write(predictors);
    // and the variables
    ioda::Variable chansVar = ogrp.vars.createWithScales<int>(
                              "channels", {ogrp.vars["nchannels"]});
    chansVar.write(channels);

    // and the number_obs_assimilated
    ioda::Variable nobsVar = ogrp.vars.createWithScales<int>(
                             "number_obs_assimilated", {ogrp.vars["nchannels"]});
    nobsVar.write(obs_assimilated);

    // Set up the creation parameters for the bias covariance coefficients variable
    ioda::VariableCreationParameters float_params;
    float_params.chunk = true;               // allow chunking
    float_params.compressWithGZIP();         // compress using gzip
    float missing_value = util::missingValue(missing_value);
    float_params.setFillValue<float>(missing_value);

    // Create a variable for bias covariance coefficients,
    // save bias covariance coeffs to the variable
    ioda::Variable anvarVar = ogrp.vars.createWithScales<float>("bias_coeff_errors",
                       {ogrp.vars["npredictors"], ogrp.vars["nchannels"]}, float_params);
    anvarVar.writeWithEigenRegular(allbcerrors);
  }
  oops::Log::trace() << "ObsBiasCovariance::write is done " << std::endl;
}

// -----------------------------------------------------------------------------

void ObsBiasCovariance::linearize(const ObsBias & bias, const eckit::Configuration & innerConf) {
  oops::Log::trace() << "ObsBiasCovariance::linearize starts" << std::endl;
  if (bias) {
    // Retrieve the QC flags and do statistics from second outer loop
    const int jouter = innerConf.getInt("iteration");
    if (jouter >= 1) {
      std::unique_ptr<ioda::Accumulator<std::vector<size_t>>> obs_num_accumulator =
          odb_.distribution()->createAccumulator<size_t>(obs_num_.size());

      // Retrieve the QC flags of previous outer loop and recalculate the number of effective obs.
      const std::string qc_group_name = "EffectiveQC" + std::to_string(jouter-1);
      const std::vector<std::string> vars = odb_.obsvariables().variables();
      std::vector<int> qc_flags(odb_.nlocs(), 999);
      for (std::size_t jvar = 0; jvar < vars.size(); ++jvar) {
        if (odb_.has(qc_group_name, vars[jvar])) {
          odb_.get_db(qc_group_name, vars[jvar], qc_flags);
          for (std::size_t jloc = 0; jloc < qc_flags.size(); ++jloc)
            if (qc_flags[jloc] == 0)
              obs_num_accumulator->addTerm(jloc, jvar, 1);
        } else {
          throw eckit::UserError("Unable to find QC flags : " + vars[jvar] + "@" + qc_group_name);
        }
      }

      // Sum across the processors
      obs_num_ = obs_num_accumulator->computeResult();

      const float missing = util::missingValue(missing);

      // compute the hessian contribution from Jo bias terms channel by channel
      // retrieve the effective error (after QC) for this channel
      const std::string err_group_name = "EffectiveError" + std::to_string(jouter-1);
      ioda::ObsVector r_inv(odb_, err_group_name);

      // compute \mathrm{R}^{-1}
      std::size_t nvars = r_inv.nvars();
      for (size_t vv = 0; vv < nvars; ++vv) {
        for (size_t ii = 0; ii < r_inv.nlocs(); ++ii) {
          if (r_inv[ii*nvars + vv] != missing) {
            r_inv[ii*nvars + vv] = 1.0f / pow(r_inv[ii*nvars + vv], 2);
          } else {
            r_inv[ii*nvars + vv] = 0.0f;
          }
        }
      }

      // compute \mathrm{H}_\beta^\intercal \mathrm{R}^{-1} \mathrm{H}_\beta
      // -----------------------------------------
      std::unique_ptr<ioda::Accumulator<std::vector<double>>> ht_rinv_h_accumulator =
          odb_.distribution()->createAccumulator<double>(ht_rinv_h_.size());
      for (std::size_t p = 0; p < prednames_.size(); ++p) {
        // retrieve the predictors
        const ioda::ObsVector predx(odb_, prednames_[p] + "Predictor");

        // for each variable
        ASSERT(r_inv.nlocs() == predx.nlocs());
        std::size_t nvars = predx.nvars();
        // only keep the diagnoal
        for (size_t vv = 0; vv < nvars; ++vv) {
          for (size_t ii = 0; ii < predx.nlocs(); ++ii)
            ht_rinv_h_accumulator->addTerm(ii,
                                           vv*prednames_.size() + p,
                                           pow(predx[ii*nvars + vv], 2) * r_inv[ii*nvars + vv]);
        }
      }

      // Sum the hessian contributions across the tasks
      ht_rinv_h_ = ht_rinv_h_accumulator->computeResult();
    }

    // reset variances for bias predictor coeff. based on current data count
    for (std::size_t j = 0; j < obs_num_.size(); ++j) {
      if (obs_num_[j] <= minimal_required_obs_number_) {
        for (std::size_t p = 0; p < prednames_.size(); ++p)
          variances_[j*prednames_.size() + p] = smallest_variance_;
      }
    }

    // set a coeff. factor for variances of control variables
    for (std::size_t j = 0; j < vars_.size(); ++j) {
      for (std::size_t p = 0; p < prednames_.size(); ++p) {
        const std::size_t index = j*prednames_.size() + p;
        preconditioner_[index] = step_size_;
        // L = \mathrm{A}^{-1}
        if (obs_num_[j] > 0)
          preconditioner_[index] = 1.0 / (1.0 / variances_[index] + ht_rinv_h_[index]);
        if (obs_num_[j] > minimal_required_obs_number_) {
          if (ht_rinv_h_[index] > 0.0) {
            analysis_variances_[index] = 1.0 / (1.0 / variances_[index] + ht_rinv_h_[index]);
          } else {
            analysis_variances_[index] = largest_analysis_variance_;
          }
        }
      }
    }
  }
  oops::Log::trace() << "ObsBiasCovariance::linearize is done" << std::endl;
}

// -----------------------------------------------------------------------------

void ObsBiasCovariance::multiply(const ObsBiasIncrement & dx1,
                                 ObsBiasIncrement & dx2) const {
  oops::Log::trace() << "ObsBiasCovariance::multiply starts" << std::endl;

  dx2.data().array() = dx1.data().array() * variances_.array();

  oops::Log::trace() << "ObsBiasCovariance::multiply is done" << std::endl;
}

// -----------------------------------------------------------------------------

void ObsBiasCovariance::inverseMultiply(const ObsBiasIncrement & dx1,
                                        ObsBiasIncrement & dx2) const {
  oops::Log::trace() << "ObsBiasCovariance::inverseMultiply starts" << std::endl;

  dx2.data().array() = dx1.data().array() / variances_.array();

  oops::Log::trace() << "ObsBiasCovariance::inverseMultiply is done" << std::endl;
}

// -----------------------------------------------------------------------------

void ObsBiasCovariance::randomize(ObsBiasIncrement & dx) const {
  oops::Log::trace() << "ObsBiasCovariance::randomize starts" << std::endl;
  if (dx) {
    static util::NormalDistribution<double> dist(variances_.size());
    for (std::size_t jj = 0; jj < variances_.size(); ++jj) {
      dx.data()[jj] = dist[jj] * std::sqrt(variances_[jj]);
    }
  }
  oops::Log::trace() << "ObsBiasCovariance::randomize is done" << std::endl;
}
// -----------------------------------------------------------------------------

std::unique_ptr<ObsBiasPreconditioner> ObsBiasCovariance::preconditioner() const {
    return std::make_unique<ObsBiasPreconditioner> (preconditioner_);
}

// -----------------------------------------------------------------------------

}  // namespace ufo


#include "db/timeline.hpp"

#include <exception>
#include <fstream>
#include <functional>
#include <boost/filesystem.hpp>

namespace fs = boost::filesystem;

namespace henhouse::db
{
    const offset_type ADD_BUCKET_BACK_LIMIT = 60;

    /**
     * This is the main function to compute the partial sums given previous bucket.
     * It turns the current non-summed bucket into a summed bucket.
     *
     * current.value is assumed to be set to the count in that bucket.
     *
     * It computes partial sum(X) and partial sum(X^2) up the current bucket.
     */
    void propogate(data_item prev, data_item& current)
    {
        const auto v = current.value;
        current.integral = prev.integral + v;
        current.second_integral = prev.second_integral + (v  * v);
    }

    //Adds a count c to the current bucket and updates the partial sum
    //values of the current bucket.
    void update_current(data_item prev, data_item& current, count_type c)
    {
        current.value += c;
        propogate(prev, current);
    }

    /**
     *
     * Mean is computing as the (running sum of x) / N.
     * In addition, variance requires maintaining the running sum of x^2
     *
     * mean = sum(x) / N
     * mean of squared x = sum(x^2) / N
     *
     * variance = (sum(x^2) / N) - (sum(x) / N )^2
     *          = (sum(x^2) / N) - mean^2
     *          = (mean of squared x) - mean^2
     *          = (mean of squared x) - (mean squared)
     */
    diff_result diff_buckets(
            const time_type ta,          //time from
            const time_type tb,          //time to
            const time_type resolution,  //resolution of time buckets
            const offset_type index_offset,
            const data_item a,           //bucket from
            const data_item b,           //bucket to
            const count_type n)          //number of buckets
    {
        REQUIRE_GREATER(resolution, 0);
        REQUIRE_GREATER(n, 0);

        //Sum here is the values added within
        const auto sum = b.integral - a.integral;
        const auto second_sum = b.second_integral - a.second_integral;
        const auto mean = static_cast<mean_type>(sum) / n;
        const auto mean_squared = mean * mean;
        const auto second_mean = static_cast<mean_type>(second_sum) / n;
        const auto variance = second_mean - mean_squared;

        return diff_result
        {
            ta,
            tb,
            resolution,
            index_offset,
            sum,
            mean,
            variance,
            n,
            a,
            b,
        };
    }

    bool timeline::put(time_type t, count_type c)
    {
        //We already have data, let's add and index new point.
        if(index.size() > 0)
        {
            const auto last_range = index.cend() - 1;

            //don't add if time is before last range
            if(t >= last_range->time)
            {
                //get last position only because we want to keep
                //a specific performance profile. This is a deliberate limitation.
                auto p = index.find_pos_from_range(t, last_range, index.cend());
                const auto pos = p.pos + p.offset;

                //bucket is current or in the past, no need to index.
                if(pos < data.size())
                {
                    //if we are too far back in the range, skip it,
                    //otherwise propogate the values up.
                    //This limitation is to keep performance predictable for
                    //inserts while providing a buffer for slow inserters
                    //to catch up.
                    if(data.size() - pos < ADD_BUCKET_BACK_LIMIT)
                    {
                        const auto prev = pos > 0 ? data[pos - 1] : data_item{0, 0, 0};
                        update_current(prev, data[pos], c);
                        for(auto p = pos + 1; p < data.size(); p++)
                            propogate(data[p-1], data[p]);
                    }
                    else return false;
                }
                //if we move beyond end, append data
                else
                {
                    const auto last_pos = data.size() - 1;
                    const auto prev = data[last_pos];

                    //don't compute integral and second_integral
                    //because propogate will overwrite
                    data_item current{c, 0, 0};
                    propogate(prev, current);
                    data.push_back(current);

                    //skip if we have no gaps, otherwise index.
                    auto new_pos = last_pos + 1;
                    if(pos == new_pos) return true;

                    //index position
                    const auto resolution = index.meta().resolution;
                    CHECK_GREATER(resolution, 0);

                    const auto aliased_time = p.time + (p.offset * resolution);
                    index_item index_entry = {aliased_time, new_pos};

                    CHECK_LESS_EQUAL(aliased_time, t);
                    index.push_back(index_entry);
                }
            }
            else return false;
        }
        //We have an empty timeline, let's add initial data point and index it.
        else
        {
            CHECK_EQUAL(data.size(), 0);

            data_item v{c, c, c * c};
            data.push_back(v);

            index_item i = {t, 0};
            index.push_back(i);
        }

        return true;
    }

    summary_result timeline::summary() const
    {
        const auto resolution = index.meta().resolution;
        CHECK_GREATER(resolution, 0);

        if(index.empty()) return summary_result{0,0,resolution, 0,0,0,0};
        REQUIRE(!data.empty());

        const auto front = index.front();
        const auto back = index.back();

        //time of first bucket
        const auto from = front.time;

        //compute time of last bucket
        CHECK_GREATER(data.size(), back.pos);
        auto last_buckets = data.size() - back.pos;
        auto to = back.time + (last_buckets * resolution);

        CHECK_GREATER(to, from);

        count_type n = (to - from) /  resolution;

        //if we have one bucket then first is empty data item
        auto first_bucket = data_item{0,0,0};
        auto last_bucket = data.back();

        //diff the two buckets
        auto diff = diff_buckets(from, to, resolution, 0, first_bucket, last_bucket, n);
        return summary_result
        {
            from,
            to,
            resolution,
            diff.sum,
            diff.mean,
            diff.variance,
            n
        };
    }

    void clamp(pos_result& r, std::size_t size)
    {
        REQUIRE_LESS(r.pos, size);

        const auto pos = r.pos + r.offset;
        if(pos < size) return;
        r.offset = size - r.pos - 1;

        ENSURE_RANGE(r.pos + r.offset, 0, size);
    }

    get_result timeline::get(time_type t, const offset_type index_offset) const
    {
        auto p = index.find_pos(t, index_offset);

        clamp(p, data.size());

        // zero out data before beginning of collection
        const bool before_beginning =  t < p.time;
        const auto dat = before_beginning ? data_item{0,0,0} : data[p.pos + p.offset];

        return get_result
        {
            p.index_offset,
                t,
                p.time,
                p.pos,
                p.offset,
                dat
        };
    }

    diff_result timeline::diff(time_type a, time_type b, const offset_type index_offset) const
    {
        const auto resolution = index.meta().resolution;
        CHECK_GREATER(resolution, 0);

        if(a > b) std::swap(a,b);
        if(data.size() == 0) return diff_result{ a, b, resolution, 0, 0, 0, 0, 0, {0}, {0}};

        auto ar = get(a, index_offset);
        auto br = get(b, index_offset);

        b = std::max(br.query_time, br.range_time);
        a = std::min(ar.query_time, b);

        const auto time_diff = b - a;
        auto n = time_diff / resolution;

        if(n == 0) return diff_result{ a, b, resolution, 0, 0, 0, 0, 0, ar.value, br.value};

        CHECK_GREATER(n , 0);
        CHECK_LESS_EQUAL(ar.index_offset, br.index_offset);
        return diff_buckets(a, b, resolution, ar.index_offset, ar.value, br.value, n);
    }

    timeline from_directory(const std::string& path, const time_type resolution)
    {
        REQUIRE(!path.empty());
        REQUIRE_GREATER(resolution, 0);

        fs::create_directory(path);
        if(!fs::is_directory(path))
            throw std::runtime_error{"path " + path + " is not a directory"};

        fs::path root = path;

        timeline t;

        fs::path idx_data = root / "_.i";
        t.index = std::move(index_type{idx_data, resolution});

        fs::path cdata = root / "_.d";
        t.data = std::move(data_type{cdata, DATA_SIZE});

        return t;
    }
}


/*
 * Copyright (c) 2011 Adrian Michel
 * http://www.amichel.com
 *
 * Permission to use, copy, modify, distribute and sell this
 * software and its documentation for any purpose is hereby
 * granted without fee, provided that both the above copyright
 * notice and this permission notice appear in all copies and in
 * the supporting documentation.
 *
 * This library is distributed in the hope that it will be
 * useful. However, Adrian Michel makes no representations about
 * the suitability of this software for any purpose.  It is
 * provided "as is" without any express or implied warranty.
 *
 * Should you find this library useful, please email
 * info@amichel.com with a link or other reference
 * to your work.
*/

#ifndef DE_CONSTRAINTS_HPP_INCLUDED
#define DE_CONSTRAINTS_HPP_INCLUDED

// MS compatible compilers support #pragma once

#if defined(_MSC_VER) && (_MSC_VER >= 1020)
#pragma once
#endif

#include <set>
#include <boost/tokenizer.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
#include <boost/lexical_cast.hpp>

#include "random_generator.hpp"
#include "de_types.hpp"

namespace de
{

/**
 * Exception thrown in case of a constraint error
 *
 * @author adrian (12/1/2011)
 */
class constraints_exception : public exception
{
public:
	/**
	 * constructor that takes the error message as argument
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param message
	 */
	constraints_exception( const std::string& message )
	: exception( message.c_str() )
	{
	}
};

/**
 * Abstract base class for concrete constraint classes
 *
 * A constraint class describes certain characteristics and
 * limits of the input variables fed to the objective function.
 *
 * @author adrian (12/1/2011)
 */
class constraint
{
public:
	virtual ~constraint(){}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double get_rand_value() = 0;

	/**
	 * returns a random value limited to the type and range of the
	 * constraint based on a previous value and an origin (see
	 * specific implementation in derived classes)
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param value
	 * @param origin
	 *
	 * @return double
	 */
	virtual double get_rand_value( double value, double origin ) = 0;

	/**
	 * returns the min limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double min() const = 0;

	/**
	 * returns the max limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double max() const = 0;

	/**
	 * Gets a random value within the limits set for the constraint,
	 * but further limited to a range defined by its origin and the
	 * width of the zone around this origin in pct of the total
	 * width
	 *
	 * @author adrian (12/13/2011)
	 *
	 * @param origin the origin (center) of the zone further
	 *  			 limiting the constraint
	 * @param zonePct the width of the zone in pct of the total
	 *  			  width, around the origin
	 *
	 * @return double
	 */
	virtual double get_rand_value_in_zone( double origin, double zonePct ) const = 0;

	/**
	 * Gets the point midway between min and max - will only work
	 * for range constraints
	 *
	 * @author adrian (12/16/2011)
	 *
	 * @return double
	 */
	virtual double get_middle_point() = 0;
};

/**
 * A smart pointer to a Constraint
 */
typedef boost::shared_ptr< constraint > constraint_ptr;

/**
 * Base class for constraints that are range based. Each such
 * constraint has a min and a max value
 *
 * @author adrian (12/1/2011)
 */
class range_constraint : public constraint
{
private:
	double m_min;
	double m_max;

public:

	/**
	 * constructor that takes the min and max limits of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param min
	 * @param max
	 */
	range_constraint( double min, double max )
	: m_min( min ), m_max( max )
	{
		assert( min <= max );
	}

	/**
	 * returns the min limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double min() const { return m_min; }

	/**
	 * returns the max limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double max() const { return m_max; }

};

/**
 * A real constraint. Specifies that variables can have any
 * double value, whitin the specified limits.
 *
 * @author adrian (12/1/2011)
 */
class real_constraint : public range_constraint
{
public:

	/**
	 * constructor that takes the min and max limit of the real
	 * constraint
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param min
	 * @param max
	 */
	real_constraint( double min, double max )
	: range_constraint( min, max )
	{
		assert( min <= max );
	}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double get_rand_value()
	{
		return genrand( range_constraint::min(), range_constraint::max() );
	}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint based on a previous value and an origin
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param value
	 * @param origin
	 *
	 * @return double
	 */
	double get_rand_value( double value, double origin )
	{
		double ret = value;

		while( ret < range_constraint::min() )
		{
			ret = range_constraint::min() + genrand() * ( origin - range_constraint::min() );
		}

		while( ret > range_constraint::max() )
		{
			ret = range_constraint::max() + genrand() * ( origin - range_constraint::max() );
		}

		return ret;
	}

	virtual double get_rand_value_in_zone( double origin, double zonePct ) const
	{
		if( origin > max() )
			throw constraints_exception( "origin coordinate > max" );
		if( origin < min() )
			throw constraints_exception( "origin coordinate < min" );

		if( zonePct > 100.0 )
			throw constraints_exception( "zonePct > 100%" );

		if( zonePct < 0 )
			throw constraints_exception( "zonePct < 0%" );

		if( zonePct == 0 )
			throw constraints_exception( "zonePct == 0%" );

		double zoneSize = ( max() - min() ) * zonePct / 100.0;

		double _min = std::max( min(), origin - zoneSize/2.0 );
		double _max = std::min( max(), origin + zoneSize/2.0 );

		return genrand( _min, _max );
	}

	virtual double get_middle_point()
	{
		return ( max() + min() )/2.0;
	}

};

/**
 * An integer constraint. Specifies that variables can have any
 * integer values within the specified limits.
 *
 * @author adrian (12/1/2011)
 */
class int_constraint : public range_constraint
{
public:
	/**
	 * constructor that takes the min and max limit of the integer
	 * constraint
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param min
	 * @param max
	 */
	int_constraint( double min, double max )
	: range_constraint( min, max )
	{
		assert( min <= max );
	}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double get_rand_value()
	{
		return genintrand( range_constraint::min(), range_constraint::max() );
	}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint based on a previous value and an origin
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param value
	 * @param origin
	 *
	 * @return double
	 */
	double get_rand_value( double value, double origin )
	{
		double ret = boost::math::round( value );

		while( ret < range_constraint::min() )
		{
			ret = range_constraint::min() + genrand() * ( origin - range_constraint::min() );
			ret = boost::math::round( ret );
		}

		while( ret > range_constraint::max() )
		{
			ret = range_constraint::max() + genrand() * ( origin - range_constraint::max() );
			ret = boost::math::round( ret );
		}

		return ret;
	}

	virtual double get_rand_value_in_zone( double origin, double zonePct ) const
	{
		if ( origin > max() )
			throw constraints_exception( "origin coordinate > max" );
		if ( origin < min() )
			throw constraints_exception( "origin coordinate < min" );

		if ( zonePct > 100.0 )
			throw constraints_exception( "zonePct > 100%" );

		if( zonePct < 0 )
			throw constraints_exception( "zonePct < 0%" );

		if( zonePct == 0  )
			throw constraints_exception( "zonePct == 0%" );

		double zoneSize = ( max() - min() ) * zonePct/100.0;

		double _min = std::max( min(), origin - zoneSize/2.0 );
		double _max = std::min( max(), origin + zoneSize/2.0 );

		double val = boost::math::round( genrand( _min, _max ) );

		for ( ;val < _min || val > _max; val = boost::math::round( genrand( _min, _max ) ) );

		return val;
	}

	virtual double get_middle_point()
	{
		return boost::math::round( ( max() - min() )/2.0 );
	}

};

/**
 * A set constraint. Specifies that variables can take any
 * values from a predefined set. Doesn't require min or max.
 *
 * Note that duplicate values will be removed
 *
 * @author adrian (12/1/2011)
 */
class set_constraint : public constraint
{
private:
	class unique : public std::unary_function < Double, bool >
	{
	public:
		bool operator ()( Double d ) const
		{
			return m_unique.insert( d ).second;
		}

	public:
		double min() const
		{
			if( m_unique.size() > 0 )
				return *m_unique.begin();
			else
				throw constraints_exception( "could not get the min value of an empty set constraint" );
		}

		double max() const
		{
			if( m_unique.size() > 0 )
				return *m_unique.rbegin();
			else
				throw constraints_exception( "could not get the max value of an empty set constraint" );
		}

	private:
		mutable std::set< Double > m_unique;
	};
private:
	unique m_unique;
	de::DVector m_values;

public:
	/**
	 * Constructs the set from a vector of Double values, and
	 * removes duplicates to ensure a uniform distribution for
	 * randomization
	 *
	 * @author adrian (12/10/2011)
	 *
	 * @param values
	 */
	set_constraint( const de::DVector& values )
	{
		// making sure the values in the "set" are unique
		std::remove_copy_if (values.begin (), values.end (), std::back_inserter( m_values ), m_unique );
	}

	/**
	 * adds an individual value to the "set", won't create duplicate
	 * values.
	 *
	 * @author adrian (12/10/2011)
	 *
	 * @param value
	 */
	void add_value( de::Double value )
	{
		if( m_unique( value ) )
			m_values.push_back( value );
	}

	/**
	 * returns a value randomly chosen from the set of available
	 * values
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double get_rand_value()
	{
		de::DVector::size_type index( genintrand( 0, m_values.size() - 1 ) );

		return m_values[ index ];
	}

	/**
	 * returns a value randomly chosen from the set of available
	 * values
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double get_rand_value(double value, double origin)
	{
		return get_rand_value();
	}

	/**
	 * returns the min limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double min() const
	{
		return m_unique.min();
	}

	/**
	 * returns the max limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double max() const
	{
		return m_unique.max();
	}

	virtual double get_rand_value_in_zone( double origin, double zonePct ) const
	{
		throw constraints_exception( "get_rand_value_in_zone only supported for range constraints" );
	}

	virtual double get_middle_point()
	{
		throw constraints_exception( "get_middle_point not supported by set constraint" );
	}

};

/**
 * A boolean constraint. Specifies that variables can take a
 * boolean value - true or false.
 *
 * @author adrian (12/1/2011)
 */
class boolean_constraint : public constraint
{
public:
	/**
	 * returns a random boolean value
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double get_rand_value()
	{
		return genrand() < 0.5;
	}

	/**
	 * returns a random boolean value
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	virtual double get_rand_value(double value, double origin)
	{
		return get_rand_value();
	}

	/**
	 * returns the min limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double min() const
	{
		return 0;
	}

	/**
	 * returns the max limit of the range
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double max() const
	{
		return 1;
	}

	virtual double get_rand_value_in_zone( double origin, double zonePct ) const
	{
		throw constraints_exception( "get_rand_value_in_zone only supported for range constraints" );
	}

	virtual double get_middle_point()
	{
		throw constraints_exception( "get_middle_point not supported by bool constraint" );
	}

};

typedef std::vector< constraint_ptr > constraints_base;

/**
 * A collection of constraints, implemented as a vector.
 *
 * Is used to define the constraints for all variables used
 * during an optimization session.
 *
 * @author adrian (12/1/2011)
 */
class constraints : public constraints_base
{
private:
	typedef boost::char_separator< char > separator;
	typedef boost::tokenizer< separator > tokenizer;

public:
	/**
	 * Initializes a collection of constraints with default values.
	 * All constraints are of type real and have the same default
	 * min and max.
	 *
	 * @author adrian (12/1/2011)
	 *
	 * @param varCount number of constraints
	 * @param defMin default min limit
	 * @param defMax default max limit
	 */
	constraints( size_t varCount, double defMin, double defMax )
	: constraints_base( varCount, boost::make_shared< real_constraint >( defMin, defMax ) )
	{
	}

	/**
	 * Initializes a collection of constraints from string
	 * descsriptions. Currently used only for range based
	 * constraints.
	 *
	 * A constraint can be described as "type;min;max" where type
	 * can be real or integer and min and max are the range limits.
	 *
	 * @author adrian (12/1/2011)
	 *
	 * @param str a a collection (vector) of constraint description
	 *  		  strings, each string describing constraints for
	 *  		  one variable
	 * @param var_count the total number of variables (can be
	 *  			   different than the number of strings). If
	 *  			   there are more variables than strings, the
	 *  			   extra constraints are set to be real and use
	 *  			   the default min and max arguments for the
	 *  			   range
	 * @param def_min default min value in case the number of
	 *  			 variables is higher than the number of
	 *  			 constraints specified as strings.
	 * @param def_max default max value in case the number of
	 *  			 variables is higher than the number of
	 *  			 constraints specified as strings.
	 */
	constraints( const std::vector< std::string >& str, size_t var_count , double def_min, double def_max )
	: constraints_base( var_count, boost::make_shared< real_constraint >( def_min, def_max ) )
	{
		for( std::vector< std::string >::size_type i = 0; i < str.size(); ++i )
		{
			tokenizer tokens( str[ i ], separator( ";," ) );

			std::string type;
			double _min;
			double _max;

			size_t count( 0 );

			for( tokenizer::const_iterator j = tokens.begin(); j != tokens.end(); ++j, ++count )
			{
				const std::string token( boost::trim_copy( *j ) );

				try
				{
					switch( count )
					{
						case 0:
							type = token;
							break;
						case 1:
							_min = boost::lexical_cast< double >( token.c_str() );
							break;
						case 2:
							_max = boost::lexical_cast< double >( token.c_str() );
							break;
						default:
							// too many fields
							throw constraints_exception( ( boost::format( "wrong variable format in \"%1%\" - too many fields" ) % str[ i ] ).str() );

					}
				}
				catch( const boost::bad_lexical_cast& )
				{
					throw constraints_exception( ( boost::format( "wrong floating point number format: %1%") % token ).str() );
				}
			}

			// too few fields
			if( count < 3 )
				throw constraints_exception( ( boost::format( "wrong variable format in \"%1%\" - too few fields" ) % str[ i ] ).str() );

			if( i < var_count )
				constraints_base::at( i ) = str_to_constraint( type, _min, _max );
			else
				constraints_base::push_back( str_to_constraint( type, _min, _max ) );
		}
	}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @return double
	 */
	double get_rand_value( size_t index )
	{
		if( index < constraints_base::size() )
			return (*this)[ index ]->get_rand_value();
		else
			throw constraints_exception( ( boost::format( "invalid constraint index: %1%, higher than max number of constraints: %2%" ) % index % constraints_base::size() ).str() );
	}

	/**
	 * returns a random value limited to the type and range of the
	 * constraint based on a previous value and an origin
	 *
	 * @author adrian (12/4/2011)
	 *
	 * @param index the constraint index
	 * @param value previous value
	 * @param origin origin
	 *
	 * @return double
	 */
	double get_rand_value( size_t index, double value, double origin )
	{
		if( index < constraints_base::size() )
			return (*this)[ index ]->get_rand_value( value, origin );
		else
			throw constraints_exception( ( boost::format( "invalid constraint index: %1%, higher than max number of constraints: %2%" ) % index % constraints_base::size() ).str() );
	}

	/**
	 * Generates a set of random values within a "hypercube" region
	 * defined by an origin and an area expressed as a percentage of
	 * the entire range
	 *
	 * @author adrian (12/13/2011)
	 *
	 * @param origin the origin coordinates of the region
	 * @param sidePct the side of the hypercube in pct of the entire
	 *  			  side
	 *
	 * @return DVectorPtr
	 */
	DVectorPtr get_square_zone_rand_values( const DVectorPtr origin, double sidePct ) const
	{
		assert( origin );
		assert( sidePct > 0 && sidePct <= 100 );

		if( origin->size() == constraints_base::size() )
		{
			DVectorPtr square( boost::make_shared< DVector >( origin->size() ) );

			for( constraints_base::size_type n = 0; n < constraints_base::size(); ++n )
				(*square)[ n ] = (*this)[ n ]->get_rand_value_in_zone( (*origin)[ n ], sidePct );

			return square;

		}
		else
			throw constraints_exception( "The origin vector must have the same number of elements as there are constraints" );
	}

	DVectorPtr get_middle_point()
	{
		DVectorPtr r( boost::make_shared< DVector >( constraints_base::size() ) );

		for( constraints_base::size_type n = 0; n < constraints_base::size(); ++n )
			(*r)[ n ] = (*this)[ n ]->get_middle_point();

		return r;

	}

	/**
	 * Get a set of random values within the limits set by the
	 * constraints
	 *
	 * @author adrian (12/13/2011)
	 *
	 * @return DVectorPtr
	 */
	DVectorPtr get_rand_values() const
	{
		DVectorPtr r( boost::make_shared< DVector >( constraints_base::size() ) );

		for( constraints_base::size_type n = 0; n < constraints_base::size(); ++n )
			(*r)[ n ] = (*this)[ n ]->get_rand_value();

		return r;
	}

private:
	constraint_ptr str_to_constraint( const std::string& type, double min, double max )
	{
		if( boost::to_lower_copy( type ) == "real" )
			return boost::make_shared< real_constraint >( min, max );
		else if( boost::to_lower_copy( type ) == "int" || boost::to_lower_copy( type ) == "integer" )
			return boost::make_shared< int_constraint >( min, max );
		else
			throw constraints_exception( ( boost::format( "invalid constraint type \"%1%\"" ) % type ).str() );
	}
};

typedef boost::shared_ptr< constraints > constraints_ptr;

}

#endif //DE_CONSTRAINTS_HPP_INCLUDED


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

// this project is translated from this project : digraph [https://github.com/damian0815/digraph]

//  Author:  MingXing Liu
//  Date:    2017-12-25
//  version:
//  v0.1
//  1.init verion

#ifndef BOOST_GRAPH_SUGIYAMA_LAYOUT_HPP
#define BOOST_GRAPH_SUGIYAMA_LAYOUT_HPP

#include <math.h>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/simple_point.hpp>
#include <boost/graph/circle_layout.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/static_assert.hpp>
#include <boost/unordered_map.hpp>
#include <boost/property_map/property_map.hpp>

namespace boost {

static const int    MAX_SWEEPS = 100;

template<typename GraphType>
static void splitIntoLayers(const GraphType& g,
                            std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                            std::map<typename graph_traits<GraphType>::vertex_iterator, int> &nodemap) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;
    typedef typename graph_traits<GraphType>::vertex_descriptor VertexDescriptor;
    typedef typename graph_traits<GraphType>::out_edge_iterator OutEdgeIter;

    typedef std::deque< VertexDescriptor > V_VEC;
    V_VEC sorted;
    topological_sort(g, std::front_inserter(sorted));

    std::map<VertexDescriptor, int> lmap;
    for (typename V_VEC::value_type &vt:sorted){
        lmap[vt] = 0;
    }

    int h = 1;
    for (typename V_VEC::iterator it = sorted.begin(); it != sorted.end(); ++it )
    {
        VertexDescriptor n1 = *it;

        OutEdgeIter out, out_end;
        tie(out, out_end) = out_edges(n1, g);
        for(; out != out_end; ++out){
            VertexDescriptor n2=target(*out,g);

            int inc = 1;
            lmap[n2] = std::max(lmap[n1] + inc, lmap[n2]);
            h = std::max(h, lmap[n2] + 1);
        }
    }

    //stack_init(h, (int)sorted.size());
    layers.resize(h);

    Viter n, nend;
    tie(n,nend) = vertices(g);
    for (typename V_VEC::value_type &vt:sorted){
        //stack_add(n+vt, lmap[vt]);
        int layerIndex=lmap[vt];

        layers[layerIndex].push_back(n+vt);
        nodemap[n+vt] = layerIndex;
    }
}

template<typename GraphType,typename IndexMap>
static void setOrderedIndexes(IndexMap &index,
                       std::vector<typename graph_traits<GraphType>::vertex_iterator>& ln) {
    for (int i = 0; i < ln.size(); i++) {
        index[*(ln[i])]=i;
    }
}

template<typename GraphType,typename RectMap>
static double maxHeight(RectMap &rect,
                 const std::vector<typename graph_traits<GraphType>::vertex_iterator> &ln) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    double mh = 0;
    for (typename std::vector<Viter>::const_iterator it = ln.begin(); it != ln.end(); ++it ) {
        Viter n = (*it);
        mh = std::max(mh, (double)(rect[*n][1]));
    }
    return mh;
}

template<typename GraphType,typename PositionMap,typename RectMap>
static double avgX(PositionMap &position,
            RectMap &rect,
            const std::vector<typename graph_traits<GraphType>::vertex_iterator>& ln) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    double m = 0;
    for (typename std::vector<Viter>::const_iterator it = ln.begin(); it != ln.end(); ++it ) {
        Viter n = *it;
        m += (position[*n][0]+rect[*n][0]/2);
    }
    return m / ln.size();
}

template<typename GraphType,typename IndexMap>
static int barycenter( IndexMap &index,
                const std::vector<typename graph_traits<GraphType>::vertex_iterator>& ln) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    if (ln.size() == 0) {
        return 0;
    }

    double bc = 0;
    for (typename std::vector<Viter>::const_iterator it = ln.begin(); it != ln.end(); ++it ) {
        Viter n = *it;

        bc += index[*n];
    }
    return (int) floor((bc / ln.size())+0.5f);
}

template<typename GraphType>
std::vector<typename graph_traits<GraphType>::vertex_iterator>
static stack_getConnectedTo(const GraphType& g,
                     std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                     typename graph_traits<GraphType>::vertex_iterator n1,
                     int layerIndex) {
    typedef typename graph_traits<GraphType>::vertex_iterator        Viter;
    typedef typename boost::graph_traits<GraphType>::edge_descriptor EdgeDescriptor;

    std::vector<Viter> ln;
    if (layerIndex < layers.size() && layerIndex >= 0) {
        for (typename std::vector<Viter>::iterator it = layers.at(layerIndex).begin(); it != layers.at(layerIndex).end(); ++it )
        {
            Viter n2 = *it;
            std::pair<EdgeDescriptor, bool> test1 = edge(*n1, *n2, g);
            if (test1.second == true) { // 判断vd1和vd2所指的两节点是否相邻
                ln.push_back(n2);
            }

            /*std::pair<EdgeDescriptor, bool> test2 = edge( *n2,*n1, g);
              if (test2.second == true) { // 判断vd1和vd2所指的两节点是否相邻
                 ln.push_back(n2);
             }*/
        }
    }
    return ln;
}

template<typename GraphType,typename IndexMap>
static void stack_reduceCrossings2L(const GraphType& g,
                             std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                             IndexMap &index,
                             int staticIndex, int flexIndex) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    assert( flexIndex < layers.size() );
    std::vector<Viter>& flex = layers[flexIndex];
    for (typename std::vector<Viter>::const_iterator it = flex.begin(); it != flex.end(); ++it )
    {
        Viter n = (*it);
        std::vector<Viter> neighbors = stack_getConnectedTo(g,layers,n, staticIndex);

        index[*n]=barycenter<GraphType>(index,neighbors);
    }

    sort( flex.begin(), flex.end(),  [&index]( Viter& n1,  Viter& n2 )->bool{
        return index[*n1]< index[*n2];
    });

    setOrderedIndexes<GraphType>(index,flex);
}

template<typename GraphType,typename PositionMap,typename RectMap>
static void stack_layerHeights(std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                        PositionMap &position,
                        RectMap &rect,
                        int &yspacing) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    double offset = 0;
    for (int l = 0; l < layers.size(); l++) {
        std::vector<Viter>& ln = layers[l];
        double maxh = maxHeight<GraphType>(rect,ln);
        for (typename std::vector<Viter>::iterator it = ln.begin(); it != ln.end(); ++it ) {
            Viter n = (*it);
            //if (n->isVirtual()) {
            if (false) {
                position[*n][1]=offset+maxHeight<GraphType>(rect,ln)/2.0;
            } else {
                position[*n][1]=offset;
            }
        }
        offset += maxh + yspacing;
    }
}

template<typename GraphType, typename PositionMap,typename RectMap>
static void stack_xPosPack(std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                    PositionMap &position,
                    RectMap &rect,
                    int flexIndex,
                    int &xspacing) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    assert( flexIndex < layers.size() );
    std::vector<Viter>& flex = layers[flexIndex];
    double offset = 0;
    for (typename std::vector<Viter>::const_iterator it = flex.begin(); it != flex.end(); ++it ) {
        Viter n = (*it);
        position[*n][0]=offset;
        offset = position[*n][0] + rect[*n][0] + xspacing;
    }
}

template<typename GraphType, typename PositionMap,typename RectMap>
static  void stack_xPosDown(const GraphType& g,
                    PositionMap &position,
                    RectMap &rect,
                    std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                    int staticIndex, int flexIndex,
                    int &xspacing) {
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    assert( flexIndex < layers.size() );
    std::vector<Viter> flex = layers[flexIndex];
    for (int i = 0; i < flex.size(); i++) {
        Viter n = flex[i];
        std::vector<Viter> neighbors = stack_getConnectedTo(g,layers,n, staticIndex);
        double avg = avgX<GraphType>(position,rect,neighbors);
        double min = (i > 0) ? (position[*(flex[i-1])][0] + rect[*(flex[i-1])][0] + xspacing) : (-std::numeric_limits<double>::max());
        if (!isnan(avg)) {
            position[*n][0]= std::max(min, avg -  rect[*n][0]/2.0);
        }
    }
}

template<typename GraphType, typename PositionMap,typename RectMap>
static  void stack_xPosUp(const GraphType& g,
                  PositionMap &position,
                  RectMap &rect,
                  std::vector<std::vector<typename graph_traits<GraphType>::vertex_iterator>> &layers,
                  int staticIndex, int flexIndex,
                  int &xspacing) {//staticIndex=flexIndex+1
    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    assert( flexIndex < layers.size() );
    std::vector<Viter> flex = layers[flexIndex];
    for (int i = flex.size() - 1; i >= 0; i--) {
        Viter n = flex[i];
        std::vector<Viter> neighbors = stack_getConnectedTo(g,layers,n, staticIndex);

        //for ( int i=0; i<neighbors.size() ;i++ )
        double avg = avgX<GraphType>(position,rect,neighbors);
        // calculate min, max
        double min = (i>0)?(position[*(flex[i-1])][0] + rect[*(flex[i-1])][0] + xspacing) : -std::numeric_limits<double>::max();
        double max =std::numeric_limits<double>::max();
        if(i<flex.size()-1){
            max=position[*(flex[i+1])][0] - rect[*n][0] - xspacing;
        }

        if (!isnan(avg)) {
            position[*n][0]=std::max(min, std::min(max, avg - rect[*n][0] / 2.0));
        }
    }
}

template<typename GraphType, typename PositionMap,typename RectMap,typename IndexMap>
void sugiyama_graph_layout(const GraphType& g,
                           PositionMap &position,
                           RectMap &rect,
                           IndexMap &index,
                           int xspacing=20,
                           int yspacing=30)
{
    BOOST_STATIC_ASSERT (property_traits<PositionMap>::value_type::dimensions >= 2);
    BOOST_STATIC_ASSERT (property_traits<RectMap>::value_type::dimensions >= 2);

    typedef typename graph_traits<GraphType>::vertex_iterator   Viter;

    typedef  std::vector<Viter>     Viter_VEC;
    typedef  std::vector<Viter_VEC> Viter_VEC_VEC;
    Viter_VEC_VEC        layers;
    std::map<Viter, int> nodemap;

    //removeCycles
    //removeCycles();

    //splitIntoLayers
    splitIntoLayers(g,layers,nodemap);

    //insertDummies
    //insertDummies();

    //initIndexes
    for (typename Viter_VEC_VEC::iterator it = layers.begin(); it != layers.end(); ++it )
    {
        Viter_VEC& l = *it;

        /*sort( l.begin(), l.end(), [&getXYWH]( Viter n1, Viter n2 )->bool{
              return getXYWH[*n1]->id < getXYWH[*n2]->id;
          });*/
        /*
          @todo figure out how to do this
          Collections.sort(l, new Comparator<Viter>() {
              public int compare(Viter n1, Viter n2) {
                  return n1.getIndex() - n2.getIndex();
              }
          });*/

        setOrderedIndexes<GraphType>(index,l);
    }

    //reduceCrossings
    for (int round = 0; round < MAX_SWEEPS; round++) {
        if (round % 2 == 0) {
            for (int l = 0; l < layers.size() - 1; l++) {
                stack_reduceCrossings2L(g,layers,index,l, l + 1);
            }
        } else {
            for (int l = layers.size() - 1; l > 0; l--) {
                stack_reduceCrossings2L(g,layers,index,l, l - 1);
            }
        }
    }

    //undoRemoveCycles
    //undoRemoveCycles();

    //layerHeights
    stack_layerHeights<GraphType>(layers,position,rect,yspacing);

    //xPos
    for (int l = 0; l < layers.size(); l++) {
        stack_xPosPack<GraphType>(layers,position,rect,l,xspacing);
    }

    printf("2\n");
    for (int l = 0; l < layers.size() - 1; l++) {
        stack_xPosDown(g,position,rect,layers,l, l + 1,xspacing);
    }
    for (int l = layers.size() - 1; l >0; l--) {
        stack_xPosUp(g,position,rect,layers,l, l - 1,xspacing);
    }
}

}

#endif


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

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

   BayesOpt 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 Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#include "bayesopt/bayesopt.hpp"

#include <boost/bind.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
//#include "randgen.hpp"
#include "lhs.hpp"
#include "gridsampling.hpp"
#include "log.hpp"

namespace bayesopt
{

  // DiscreteModel::DiscreteModel(const vecOfvec &validSet):
  //   BayesOptBase(), mInputSet(validSet)
  // {} // Constructor

  DiscreteModel::DiscreteModel( const vecOfvec &validSet,
				Parameters parameters):
    BayesOptBase(validSet[0].size(),parameters), mInputSet(validSet)
  {
    mDims = mInputSet[0].size();
  } // Constructor

  DiscreteModel::DiscreteModel(const vectori &categories,
			       Parameters parameters):
   BayesOptBase(categories.size(),parameters)
  {
    mDims = categories.size();
    utils::buildGrid(categories,mInputSet);
  }

  DiscreteModel::~DiscreteModel()
  {} // Default destructor

  // PROTECTED
  vectord DiscreteModel::samplePoint()
  {
    randInt sample(mEngine, intUniformDist(0,mInputSet.size()-1));
    return mInputSet[sample()];
  };

  void DiscreteModel::findOptimal(vectord &xOpt)
  {
    std::vector<double> critv(mInputSet.size());
    std::transform(mInputSet.begin(),mInputSet.end(),critv.begin(),
		   boost::bind(&DiscreteModel::evaluateCriteria,this,_1));

    xOpt = mInputSet[std::distance(critv.begin(),
			 std::max_element(critv.begin(),critv.end()))];

    // xOpt = *mInputSet.begin();
    // double min = evaluateCriteria(xOpt);

    // for(vecOfvec::iterator it = mInputSet.begin();
    // 	it != mInputSet.end(); ++it)
    //   {
    // 	double current = evaluateCriteria(*it);
    // 	if (current < min)
    // 	  {
    // 	    xOpt = *it;
    // 	    min = current;
    // 	  }
    //   }
  }

  //In this case, it is the trivial function
  vectord DiscreteModel::remapPoint(const vectord& x)
  { return x; }

  void DiscreteModel::generateInitialPoints(matrixd& xPoints)
  {

    vecOfvec perms = mInputSet;

    // By using random permutations, we guarantee that
    // the same point is not selected twice
    utils::randomPerms(perms,mEngine);

    // vectord xPoint(mInputSet[0].size());
    for(size_t i = 0; i < xPoints.size1(); i++)
    {
        const vectord xP = perms[i];
        row(xPoints,i) = xP;
    }
  }

}  // namespace bayesopt


// **************************************************************************/
//  作者: 陈雪飞<chenxuefei_pp@163.com>
//  日期: 2017年1月15日  14:53:16
//  工程: OpticalFlow
//  程序: OpticalFlow
//  文件: OpticalFlow.cpp
//  描述: 光流法裂纹检测
// **************************************************************************/

#include <opencv2/opencv.hpp>
#include <boost/thread.hpp>

//基本类型定义
//PI定义
const static double  PI = 3.14159265358979323846;
//特征点个数定义
const static uint    FEATURE_MAX_NUM = 600;
//图像二值化阈值
const static double  IMAGE_THRESHOLD = 127.00f;

/**
* \brief 创建窗口线程函数，将窗口独立到线程中
*/
void    output_windows_thread(const std::string &windowname)
{
    cv::namedWindow(windowname);
    cv::waitKeyEx();
}

/**
* \brief 定义平方函数
* \tparam _Ty 平方类型
* \param a 平方因子
* \return
*/
template<typename _Ty>
double square(_Ty a)
{
    return a * a;
}

/**
* \brief 两点之间线段长度
* \param p1 点向量1
* \param p2 点向量2
* \return 计算出的线段长度
*/
double  segment_length(const cv::Point & p1, const cv::Point &p2)
{
    return std::sqrt(square(p1.x - p2.x) + square(p1.y - p2.y));
}

/**
* \brief 定义图像处理函数
* \param name 处理方法名
*/
#define     DEF_IMG_HANDLER(name)   \
    void  name(cv::Mat &in,cv::Mat &out)

/**
* \brief 定义处理操作符
* \tparam Functor 处理方法
* \param img 处理图像
* \param f 方法
* \return 处理后的图像
*/
template <typename Functor>
cv::Mat  &operator << (cv::Mat &img, Functor f)
{
    f(img, img);
    return img;
}

/**
* \brief 灰度化处理方法
* \param in 原图像三通道
* \param out 输出图像，灰度图像
*/
DEF_IMG_HANDLER(cvtgray)
{
    assert(in.channels() == 3);
    cv::cvtColor(in, out, cv::COLOR_BGR2GRAY);
}

/**
* \brief 灰度图像转彩色图像
* \param in 灰度图像，单通道
* \param out 彩色图像，三通道
*/
DEF_IMG_HANDLER(cvtbgr)
{
    assert(in.channels() == 1);
    cv::cvtColor(in, out, cv::COLOR_GRAY2BGR);
}

/**
* \brief 图像转HSV格式
* \param in 源图像
* \param out hsv格式图像
*/
DEF_IMG_HANDLER(cvthsv)
{
    cv::cvtColor(in, out, cv::COLOR_BGR2HSV);
}

/**
* \brief 二值化图像
* \param in 传入图像
* \param out 二值化图像，单通道
*/
DEF_IMG_HANDLER(binary)
{
    //cv::threshold(in, out, IMAGE_THRESHOLD, 255.0f, CV_THRESH_BINARY);

    cv::adaptiveThreshold(in, out, 255.0f,
        cv::AdaptiveThresholdTypes::ADAPTIVE_THRESH_GAUSSIAN_C,
        cv::ThresholdTypes::THRESH_BINARY, 7, 0.0);
}

/**
* \brief 白平衡处理，均衡化三通道
* \param in 输入图像三通道
* \param out 输出图像三通道
*/
DEF_IMG_HANDLER(whiteblace)
{
    assert(in.channels() == 3);
    using namespace cv;
    Mat &g_srcImage = in, &dstImage = out;
    std::vector<Mat> g_vChannels;

    split(g_srcImage, g_vChannels);
    Mat imageBlueChannel = g_vChannels.at(0);
    Mat imageGreenChannel = g_vChannels.at(1);
    Mat imageRedChannel = g_vChannels.at(2);

    double imageBlueChannelAvg = 0;
    double imageGreenChannelAvg = 0;
    double imageRedChannelAvg = 0;

    //求各通道的平均值
    imageBlueChannelAvg = mean(imageBlueChannel)[0];
    imageGreenChannelAvg = mean(imageGreenChannel)[0];
    imageRedChannelAvg = mean(imageRedChannel)[0];

    //求出个通道所占增益
    double K = (imageRedChannelAvg + imageGreenChannelAvg + imageRedChannelAvg) / 3;
    double Kb = K / imageBlueChannelAvg;
    double Kg = K / imageGreenChannelAvg;
    double Kr = K / imageRedChannelAvg;

    //更新白平衡后的各通道BGR值
    addWeighted(imageBlueChannel, Kb, 0, 0, 0, imageBlueChannel);
    addWeighted(imageGreenChannel, Kg, 0, 0, 0, imageGreenChannel);
    addWeighted(imageRedChannel, Kr, 0, 0, 0, imageRedChannel);

    merge(g_vChannels, dstImage);//图像各通道合并
}

/**
* \brief 灰度直方图均衡化
* \param in 输入灰度图
* \param out 输出灰度图
*/
DEF_IMG_HANDLER(equalizeHist)
{
    assert(in.channels() == 1);
    cv::equalizeHist(in, out);
}

/**
* \brief 图像锐化，拉普拉斯算子
* \param in 输入图像
* \param out 输出图像
*/
DEF_IMG_HANDLER(sharpen)
{
    cv::Mat kernela(3, 3, CV_32F, cv::Scalar(0));
    kernela.at<float>(1, 1) = 5.0;
    kernela.at<float>(0, 1) = -1.0;
    kernela.at<float>(2, 1) = -1.0;
    kernela.at<float>(1, 0) = -1.0;
    kernela.at<float>(1, 2) = -1.0;
    filter2D(in, out, in.depth(), kernela);
}

/**
* \brief 对图像进行预处理
* \param base_image 待预处理的图像
*/
void handle_base_out_pre(cv::Mat& base_image)
{
    std::vector<cv::Mat> mv;

    //分离红色通道分量
    cv::split(base_image, mv);
    auto &red = mv.at(2);

    /// 膨胀操作
    int dilation_size = 1;
    auto element = getStructuringElement(cv::MORPH_RECT,
        cv::Size(2 * dilation_size + 1, 2 * dilation_size + 1),
        cv::Point(dilation_size, dilation_size));
    dilate(red, red, element);
    dilate(red, red, element);
    dilate(red, red, element);
    //将分量组合
    cv::merge(mv, base_image);

    cv::imshow("superposition_window", base_image);

    cv::imwrite("叠加膨胀结果.png", base_image);
}
/**
* \brief 查找最大连通域
* \param base_image
*/
void handle_base_out_last(const cv::Mat& base_image, cv::Mat & backend_image)
{
    //最大面积区域需要显示几个
    const static  auto area_number = 3;

    cv::Mat		new_image = backend_image;

    assert(!new_image.empty());//检查是否为空

    std::vector<cv::Vec4i>          hierarchy;
    std::vector<cv::Mat>            contours;

    cv::Mat input;
    base_image.copyTo(input);

    cv::cvtColor(input, input, cv::COLOR_BGR2GRAY);

    input << binary;//二值化
    cv::imwrite("二值结果.png", input);

    cv::findContours(input, contours, hierarchy,
        cv::RetrievalModes::RETR_CCOMP,
        cv::ContourApproximationModes::CHAIN_APPROX_SIMPLE);

    //输出查找到的所有轮廓
    {
        cv::Mat all_outline;
        new_image.copyTo(all_outline);
        cv::drawContours(all_outline, contours, -1, CV_RGB(0, 255, 255), 1 , cv::LINE_AA);
        cv::imwrite("查找到的所有轮廓.png", all_outline);
    }

    /// Find the convex hull object for each contour
    std::vector<std::vector<cv::Point2i> >	hull(contours.size()), dest_hull;
    // Int type hull
    std::vector<std::vector<int>>	hullsI(contours.size());
    // Convexity defects
    std::vector<cv::Mat>			defects(contours.size());

    //凹点还原
    for (size_t i = 0; i < contours.size(); i++)
    {
        cv::convexHull(contours[i], hull[i], false);
        // find int type hull
        cv::convexHull(contours[i], hullsI[i], false);
        // get convexity defects
        convexityDefects(contours[i], hullsI[i], defects[i]);
    }

    //查找最大轮廓
    for (auto i = 0; i < area_number; ++i)
    {
        auto max_contours = std::max_element(hull.begin(), hull.end(), [](const std::vector<cv::Point> &l, const std::vector<cv::Point> &r)->bool
        {
            auto area1 = fabs(cv::contourArea(l));
            auto area2 = fabs(cv::contourArea(r));
            return area1 < area2;
        });
        dest_hull.push_back(*max_contours);
        hull.erase(max_contours);
    }

    cv::drawContours(new_image, dest_hull, -1, CV_RGB(0, 255, 255), 1 , cv::LINE_AA);

    //cv::drawContours(base_image, contours ,-1, CV_RGB(255, 0, 0),1,cv::LINE_AA);
    //根据凹点还原在原图连线，表示出最大连通点
    //for (size_t i = 0; i< contours.size(); i++)
    //{
    //	cv::Scalar color = CV_RGB(0,255,0);
    //	drawContours(new_image, hull, i, color, 1, 8, std::vector<cv::Vec4i>(), 0, cv::Point2i());

    //	//auto d = defects[i].begin<cv::Vec4i>();
    //	//while (d != defects[i].end<cv::Vec4i>()) {
    //	//	cv::Vec4i& v = (*d);
    //	//	//if(IndexOfBiggestContour == i)
    //	//	{

    //	//		int startidx = v[0];
    //	//		cv::Point2i ptStart(contours[i].at<cv::Point2i>(startidx)); // point of the contour where the defect begins
    //	//		int endidx = v[1];
    //	//		cv::Point2i ptEnd(contours[i].at<cv::Point2i>(endidx)); // point of the contour where the defect ends
    //	//		int faridx = v[2];
    //	//		cv::Point2i ptFar(contours[i].at<cv::Point2i>(faridx));// the farthest from the convex hull point within the defect
    //	//		int depth = v[3] / 256; // distance between the farthest point and the convex hull

    //	//		if (depth > 20 && depth < 80)
    //	//		{
    //	//			//line(new_image, ptStart, ptFar, CV_RGB(0, 255, 0), 2);
    //	//			//line(new_image, ptEnd, ptFar, CV_RGB(0, 255, 0), 2);
    //	//			/*circle(base_image, ptStart, 4, cv::Scalar(255, 0, 100), 2);
    //	//			circle(base_image, ptEnd, 4, cv::Scalar(255, 0, 100), 2);
    //	//			circle(base_image, ptFar, 4, cv::Scalar(100, 0, 255), 2);*/
    //	//		}
    //	//	}
    //	//	++d;
    //	//}
    //}
    //cv::drawContours(base_image, defects, -1, CV_RGB(0, 255, 0), 1, cv::LINE_AA);
    cv::imshow("output_window", new_image);
    cv::imwrite("最终结果.png", new_image);
}
/**
* \brief 开始绘制运动轨迹 如需要
* \param m1 基本矩阵
* \param frame1_features 查找到的第一帧特征点
* \param frame2_features 查找到的第二帧特征点
* \param found_features 特征点错误矩阵
*/
void    draw_path_on_baseimage(cv::Mat &m1, const cv::Mat &frame1_features, const cv::Mat &frame2_features, const cv::Mat &found_features)
{
    const static cv::Scalar line_color = CV_RGB(255, 0, 0);

    for (auto pos = 0; pos < FEATURE_MAX_NUM; ++pos)
    {
        /* 如果没找到对应特征点 */
        if (found_features.at<uchar>(pos) == 0)
            continue;

        auto p = frame1_features.at<cv::Point2f>(pos);
        auto q = frame2_features.at<cv::Point2f>(pos);

        if (segment_length(p, q) > 20.0f)
            return;

        double angle;
        angle = atan2(static_cast<double>(p.y) - q.y, static_cast<double>(p.x) - q.x);
        double hypotenuse;
        hypotenuse = sqrt(square(p.y - q.y) + square(p.x - q.x));

        /*执行缩放*/
        q.x -= 5 * hypotenuse * cos(angle);
        q.y -= 5 * hypotenuse * sin(angle);

        /*画箭头主线*/
        cv::line(m1, p, q, line_color, 1, cv::LineTypes::LINE_AA, 0);

        /* 画箭的头部*/
        p.x = q.x + 9 * cos(angle + PI / 4);
        p.y = q.y + 9 * sin(angle + PI / 4);
        cv::line(m1, p, q, line_color, 1, cv::LineTypes::LINE_AA, 0);
        p.x = q.x + 9 * cos(angle - PI / 4);
        p.y = q.y + 9 * sin(angle - PI / 4);
        cv::line(m1, p, q, line_color, 1, cv::LineTypes::LINE_AA, 0);
    }
}
/**
* \brief 将特征点信息绘制到基础背景图像中去
* \param base_image 要绘制的图像
* \param frame1_features 第一帧特征点
* \param frame2_features 第二帧特征点
* \param found_features 特征点信息
*/
void draw_features_in_base_image(cv::Mat& base_image, const cv::Mat& frame1_features, const cv::Mat& frame2_features, const cv::Mat& found_features)
{
    const static cv::Scalar area_color = CV_RGB(255, 0, 0);

    for (auto pos = 0; pos < FEATURE_MAX_NUM; ++pos)
    {
        /* 如果没找到对应特征点 */
        if (found_features.at<uchar>(pos) == 0)
            continue;
        auto start_p = frame1_features.at<cv::Point2f>(pos);
        auto end_p = frame2_features.at<cv::Point2f>(pos);
        //如果线段长度大于某个值，说明不是细微运动则排除该线段
        if (segment_length(start_p, end_p) > 20.0f)
            return;

        cv::line(base_image, start_p, end_p, area_color, 3, cv::LINE_AA);
    }
}
/**
* \brief 打开视频文件并返回帧数
* \param capture 视频句柄
* \param filename 文件名
* \return 返回帧数
*/
int open_video(cv::VideoCapture & capture, const std::string &filename)
{
    //打开视频文件
    capture.open(filename);
    auto  frame_count = 0; //总帧长

    assert(capture.isOpened());//检查是否打开

    //获取总帧数
    frame_count = static_cast<int>(capture.get(cv::VideoCaptureProperties::CAP_PROP_FRAME_COUNT));

    //进度条，可以查看处理了多少帧
    return frame_count;
}

/**
* \brief 开始LK光流法算法
* \param base_out 叠加输出基本源图像
*/
void draw_features_by_lk(cv::Mat & base_out, const std::string & in_video_name, const std::string & out_video_name)
{
    //打开视频文件并且创建进度条
    cv::VideoCapture capture{};
    auto frame_count = open_video(capture, in_video_name);
    //boost::progress_display progress_bar(frame_count, std::cout, "当前处理进度\n\t\t", "总进度\t\t", "当前进度\t");
    //打开输出视频文件 如需要
    cv::VideoWriter writer(out_video_name, CV_FOURCC('X', 'V', 'I', 'D'), 29.0, cv::Size{ 656,488 });

    //如果还有新帧则继续
    while (capture.grab())
    {
        cv::Mat     m1, m2, frame1, frame2;
        //取出视频两帧
        capture >> m1;
        capture >> m2;
        //如果返回不足两帧则退出
        if (m1.empty() || m2.empty())
            break;
        //复制到新空间
        frame1 = m1;
        frame2 = m2;
        //两帧灰度化
        frame1 << cvtgray;
        frame2 << cvtgray;

        //创建两个特征值存储矩阵
        cv::Mat frame1_features, frame2_features;
        //开始进行特征值检测，先检测出第一帧特征值
        cv::goodFeaturesToTrack(frame1, frame1_features, FEATURE_MAX_NUM, .01, .01);
        //创建查找错误矩阵
        cv::Mat found_futures, found_err;
        //创建迭代算子
        cv::TermCriteria optical_flow_termination_criteria(cv::TermCriteria::Type::MAX_ITER | cv::TermCriteria::Type::EPS, 20, .3);
        //开始进行LK光流法计算，查找第二个特征值
        cv::calcOpticalFlowPyrLK(
            frame1, frame2,
            frame1_features, frame2_features,
            found_futures, found_err,
            cv::Size{ 5,5 }, 5, optical_flow_termination_criteria);
        //绘制移动轨迹 ， 如需要
        draw_path_on_baseimage(m1, frame1_features, frame2_features, found_futures);
        //绘制运动轨迹线到背景矩阵
        draw_features_in_base_image(base_out, frame1_features, frame2_features, found_futures);

        //进度条更新
        //progress_bar += 2;

        //将运动轨迹线展示到窗口，如需要
        cv::imshow("output_window", m1);
        cv::imshow("superposition_window", base_out);
        //将运动轨迹线输出到视频文件 如需要
        writer << m1;
        //writer << (out << cvtbgr);
    }

    writer.release();
    capture.release();
}

/**
* \brief 主函数
* \return
*/
int main(int argc, char ** args)
{
    if (argc < 3)
    {
        std::cout << "请输入输入视频文件名,输入背景图像文件名,(或者输出视频文件名),按顺序空格分离." << std::endl;
        std::cout << "Example:\n\tOpticalFlow 输入视频.avi 输入背景.jpg [输出视频.avi]" << std::endl;
        exit(-1);
    }
    std::string input_video_filename = args[1];
    std::string base_image_filename = args[2];
    std::string base_output_filename = "VideoOut.avi";
    if (argc == 4)
        base_output_filename = args[3];

    //创建窗口
    auto  output_window_fu = boost::thread(output_windows_thread, "output_window");
    auto  superposition_window_fu = boost::thread(output_windows_thread, "superposition_window");

    //boost::thread(output_windows_thread).detach();
    //打开背景图像
    auto            base_image = cv::imread(base_image_filename);
    //打开是否失败
    assert(!base_image.empty());

    //创建空白背景图像
    cv::Mat         base_out{ base_image.rows,base_image.cols,base_image.type() };
    //cv::Mat          base_out = cv::imread("post.png");
    //开始进行LK光流算法，将结果图像输出到base out
    draw_features_by_lk(base_out, input_video_filename, base_output_filename);

    /*
    * ！！！！此处开始对LK算法处理后的图像进行处理！！！！
    */
    //对叠加输出图片进行预处理
    handle_base_out_pre(base_out);
    //对预处理图片进行整合处理，边缘过滤，凹点过滤
    handle_base_out_last(base_out, base_image);

    //等待窗口程序结束
    superposition_window_fu.join();
    output_window_fu.join();
    return 0;
}


/*******************************************************************************
*
*  Filename    : PlotSimFitVal.cc
*  Description : Plotting Pull distribution and others
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "TstarAnalysis/LimitCalc/interface/Common.hpp"
#include "TstarAnalysis/LimitCalc/interface/SimFitVal.hpp"

#include "ManagerUtils/Maths/interface/RooFitExt.hpp"
#include "ManagerUtils/PlotUtils/interface/Common.hpp"
#include "ManagerUtils/PlotUtils/interface/RooFitUtils.hpp"
#include "ManagerUtils/SampleMgr/interface/SampleMgr.hpp"
#include "TstarAnalysis/Common/interface/PlotStyle.hpp"

#include <algorithm>
#include <boost/format.hpp>
#include <fstream>

#include "RooDataSet.h"
#include "RooGaussian.h"
#include "RooRealVar.h"
#include "TGraphAsymmErrors.h"
#include "TMultiGraph.h"

using namespace std;
using namespace mgr;

/*******************************************************************************
*   Declaring global static variables
*******************************************************************************/
static RooRealVar p( "p", "p", 0, -7, 7 );

/*******************************************************************************
*   Defining datatype
*******************************************************************************/
const unsigned BKG_IDX = 0;
const unsigned SIG_IDX = 1;
const unsigned P1_IDX  = 2;
const unsigned P2_IDX  = 3;

/*******************************************************************************
*   Defining main control flows
*******************************************************************************/
void
PlotGenFit( const vector<string>& masslist )
{

  map<int, PullResult> pullresults;

  p.setRange( "reduce", -2, 2 );

  for( const auto& masspoint : masslist ){
    const int mass = GetInt( masspoint );
    pullresults[mass] = PlotSingleGenFit( masspoint );
    cout << mass << endl;
  }

  MakePullComparePlot( pullresults, BKG_IDX, "bkg"    );
  MakePullComparePlot( pullresults, SIG_IDX, "sig"    );
  // MakePullComparePlot( pullresults, P1_IDX ,"param1" );
  // MakePullComparePlot( pullresults, P2_IDX ,"param2" );

}

/******************************************************************************/

PullResult
PlotSingleGenFit( const std::string& masstag )
{
  const string strgthtag = SigStrengthTag();
  const string rhotag    = RhoTag();
  const string filename  = limnamer.TextFileName( "valsimfit", masstag, rhotag, strgthtag );
  ifstream result( filename );

  RooDataSet bkgset( "bkg", "bkg", RooArgSet( p ) );
  RooDataSet sigset( "sig", "sig", RooArgSet( p ) );
  RooDataSet p1set( "p1", "p1", RooArgSet( p ) );
  RooDataSet p2set( "p2", "p2", RooArgSet( p ) );
  RooDataSet signormset( "signorm", "signorm", RooArgSet( p ) );
  double bkg_real, sig_real, p1_real, p2_real;
  double bkg_fit, bkg_fiterr;
  double sig_fit, sig_fiterr;
  double p1_fit, p1_fiterr;
  double p2_fit, p2_fiterr;

  PullResult ans;

  // Begin reading files
  cout << "Reading file " << filename << endl;
  result >> bkg_real >> sig_real >> p1_real >> p2_real;

  while( result >> bkg_fit >>  bkg_fiterr >> sig_fit >>  sig_fiterr >> p1_fit >>  p1_fiterr >> p2_fit >>  p2_fiterr ){

    // Making pull distribution
    double bkgpull = ( bkg_fit - bkg_real ) / bkg_fiterr;
    double sigpull = ( sig_fit - sig_real ) / sig_fiterr;
    double p1pull  = ( p1_fit  - p1_real  ) / p1_fiterr;
    double p2pull  = ( p2_fit  - p2_real  ) / p2_fiterr;
    double signorm = ( sig_fit - sig_real ) / sig_real;

    if(
      bkgpull < -7 || bkgpull > +7 ||
      sigpull < -7 || sigpull > +7 ||
      p1pull  < -7 || p1pull  > +7 ||
      p2pull  < -7 || p2pull  > +7
      ){ continue; }

    p = bkgpull;
    bkgset.add( RooArgSet( p ) );
    p = sigpull;
    sigset.add( RooArgSet( p ) );
    p = p1pull;
    p1set.add( RooArgSet( p ) );
    p = p2pull;
    p2set.add( RooArgSet( p ) );
    p = signorm;
    signormset.add( RooArgSet( p ) );
  }

  ans.fitparm[BKG_IDX] = MakePullPlot( bkgset, masstag, "bkg"    );
  ans.fitparm[SIG_IDX] = MakePullPlot( sigset, masstag, "sig"    );
  ans.fitparm[P1_IDX]  = MakePullPlot( p1set,  masstag, "param1" );
  ans.fitparm[P2_IDX]  = MakePullPlot( p2set,  masstag, "param2" );

  return ans;
}

/*******************************************************************************
*   Plotting functions
*******************************************************************************/
pair<Parameter, Parameter>
MakePullPlot( RooDataSet& set, const string& masstag, const string& tag )
{
  RooRealVar mean( "m", "m", 0, -5, 5 );
  RooRealVar sigma( "s", "s", 1,  0, 5 );
  RooGaussian pullfit( "pull", "pull", p, mean, sigma );
  RooRealVar smean( "sm", "sm", 0, -5, 5 );
  RooRealVar ssigma( "ss", "ss", 1,  0, 5 );
  RooGaussian spullfit( "spull", "spull", p, smean, ssigma );
  p.setRange( "reduce", -2, 2 );

  TCanvas* c = mgr::NewCanvas();

  pullfit.fitTo( set,
    RooFit::Minimizer( "Minuit", "Migrad" ),
    RooFit::Minos( kTRUE ),
    RooFit::Verbose( kFALSE ),
    RooFit::PrintLevel( -1 ),
    RooFit::PrintEvalErrors( -1 ),
    RooFit::Warnings( kFALSE )
    );

  spullfit.fitTo( set,
    RooFit::Minimizer( "Minuit", "Migrad" ),
    RooFit::Minos( kTRUE ),
    RooFit::Verbose( kFALSE ),
    RooFit::PrintLevel( -1 ),
    RooFit::PrintEvalErrors( -1 ),
    RooFit::Warnings( kFALSE ),
    RooFit::Range( "reduce" )// Fitting to sub-range
    );

  RooPlot* frame  = p.frame();
  TGraph* setplot = mgr::PlotOn( frame, &set,
    RooFit::Binning( 39, p.getMin(), p.getMax() ),
    RooFit::DrawOption( PGS_DATA )
    );
  TGraph* fitplot = mgr::PlotOn( frame, &pullfit );
  TGraph* subplot = mgr::PlotOn( frame, &spullfit, RooFit::Range( "reduce" ) );
  frame->Draw();

  // Styling
  tstar::SetDataStyle( setplot );
  fitplot->SetLineColor( KBLUE );
  subplot->SetLineColor( KRED  );

  const double ymax = mgr::GetYmax( setplot, fitplot, subplot );
  frame->SetMaximum( ymax * 1.6 );
  frame->GetYaxis()->SetTitle("Events");

  // Calculation Kolmogorov-Smirnov Goodness-of-Fit Test
  const double ksres  = KSTest( set, pullfit, p );
  const double sksres = KSTest( set, spullfit, p, RooFit::CutRange( "reduce" ) );

  // Styling plots
  mgr::SetFrame( frame );
  mgr::DrawCMSLabelOuter( SIMULATION );
  mgr::DrawLuminosity( mgr::SampleMgr::TotalLuminosity() );

  // Title Formats
  boost::format xtitlefmt( "pull_{%s}" );
  boost::format signalfmt( "Sig. mass = %d GeV/c^{2}" );
  boost::format fitfmt(  "#mu = %.2lf_{#pm%.3lf}  #sigma = %.3lf_{#pm%.3lf}"  );
  boost::format sfitfmt( "#mu' = %.2lf_{#pm%.3lf} #sigma' = %.3lf_{#pm%.3lf}"  );
  boost::format ksfmt( "K = %.3lf" );
  boost::format injectfmt( "signal events %1% %2%" );

  const string xtitle_s = str( xtitlefmt % tag );
  const string signal_s = str( signalfmt % GetInt( masstag ) );
  const string fit_s    = str( fitfmt    % mean.getVal() % mean.getError() % sigma.getVal() % sigma.getError() );
  const string sfit_s   = str( sfitfmt   % smean.getVal()% smean.getError()%ssigma.getVal() %ssigma.getError() );
  const string inject_s = limnamer.CheckInput( "relmag" ) ? boost::str( injectfmt % 'x' % limnamer.GetInput<double>( "relmag" ) ) :
                          boost::str( injectfmt % '=' % limnamer.GetInput<double>( "absmag" ) );

  // Making titles
  frame->SetTitle( "" );
  frame->GetXaxis()->SetTitle( xtitle_s.c_str() );

  // Legend for marking
  TLegend* tl = mgr::NewLegend( 0.5, 0.6 );
  tl->AddEntry( fitplot,        fit_s.c_str(),                     "l" );
  tl->AddEntry( (TObject*)NULL, boost::str( ksfmt%ksres ).c_str(),  "" );
  tl->AddEntry( subplot,       sfit_s.c_str(),                     "l" );
  tl->AddEntry( (TObject*)NULL, boost::str( ksfmt%sksres ).c_str(), "" );
  tl->Draw();

  LatexMgr latex;
  latex.SetOrigin( PLOT_X_TEXT_MIN, PLOT_Y_TEXT_MAX, TOP_LEFT );
  latex.WriteLine( limnamer.GetChannelEXT( "Root Name" ) );
  latex.WriteLine( limnamer.GetExt<string>( "fitfunc", "Full Name" ) );
  latex.WriteLine( signal_s );
  latex.WriteLine( inject_s );

  mgr::SaveToPDF( c, limnamer.PlotFileName( "valpulldist", RhoTag(), masstag,  tag,  SigStrengthTag() ) );

  delete frame;
  delete c;

  return pair<Parameter, Parameter>( mean, sigma );
}

void
MakePullComparePlot(
  const map<int, PullResult>& pullresultlist,
  const unsigned idx,
  const string& tag
  )
{
  TCanvas* c = mgr::NewCanvas();
  c->SetLeftMargin( PLOT_X_MIN );
  c->SetRightMargin( 1 - PLOT_X_MAX );
  c->SetBottomMargin( PLOT_Y_MIN );
  c->SetTopMargin( 1 - PLOT_Y_MAX );

  TGraphAsymmErrors* graph   = new TGraphAsymmErrors( pullresultlist.size() );
  TGraphAsymmErrors* meanerr = new TGraphAsymmErrors( pullresultlist.size() );
  TGraphAsymmErrors* uperr   = new TGraphAsymmErrors( pullresultlist.size() );
  TGraphAsymmErrors* lowerr  = new TGraphAsymmErrors( pullresultlist.size() );

  unsigned i = 0;

  for( const auto& resultpair : pullresultlist ){
    const int mass          = resultpair.first;
    const PullResult result = resultpair.second;
    const Parameter fitmean = result.fitparm[idx].first;
    const Parameter fitsig  = result.fitparm[idx].second;

    graph->SetPoint( i, mass, fitmean.CentralValue() );
    meanerr->SetPoint( i, mass, fitmean.CentralValue() );
    uperr->SetPoint( i, mass, fitmean.CentralValue() + fitsig.CentralValue() );
    lowerr->SetPoint( i, mass, fitmean.CentralValue() - fitsig.CentralValue() );

    graph->SetPointError( i, 0, 0, fitsig.CentralValue(), fitsig.CentralValue() );
    meanerr->SetPointError( i, 0, 0, fabs( fitmean.AbsLowerError() ), fabs( fitmean.AbsUpperError() ) );
    uperr->SetPointError( i, 0, 0, fabs( fitsig.AbsLowerError() ), fabs( fitsig.AbsUpperError() ) );
    lowerr->SetPointError( i, 0, 0, fabs( fitsig.AbsUpperError() ), fabs( fitsig.AbsLowerError() ) );
    ++i;
  }

  TMultiGraph* mg = new TMultiGraph();
  mg->Add( meanerr, "A3" );
  mg->Add( uperr,   "A3" );
  mg->Add( lowerr,  "A3" );
  mg->Add( graph,   "LP" );

  mg->Draw( "A" );
  mg->SetTitle( "" );
  mg->GetXaxis()->SetTitle( "Signal mass (GeV/c^{2})" );
  mg->GetYaxis()->SetTitle( ( "pull_{"+tag+"}" ).c_str() );
  mg->SetMaximum( 3.25 );
  mg->SetMinimum( -1.75 );
  mgr::SetAxis( mg );
  mgr::DrawCMSLabel( SIMULATION );
  mgr::DrawLuminosity( mgr::SampleMgr::TotalLuminosity() );

  // Styling graphs
  graph->SetMarkerStyle( 21 );
  meanerr->SetFillStyle( 1001 );
  meanerr->SetFillColor( kBlue );
  meanerr->SetLineColor( kBlue );
  uperr->SetFillStyle( 1001 );
  uperr->SetFillColor( kCyan );
  uperr->SetFillColor( kCyan );
  lowerr->SetFillStyle( 1001 );
  lowerr->SetFillColor( kCyan );
  lowerr->SetFillColor( kCyan );

  const double xmin = mg->GetXaxis()->GetXmin();
  const double xmax = mg->GetXaxis()->GetXmax();
  TLine z( xmin, 0, xmax, 0 );
  TLine hi( xmin, 1, xmax, 1 );
  TLine lo( xmin, -1, xmax, -1 );
  z.SetLineColor( kRed );
  z.SetLineWidth( 3 );
  z.SetLineStyle( 2 );
  z.Draw();
  hi.SetLineColor( kBlue );
  hi.SetLineWidth( 2 );
  hi.SetLineStyle( 3 );
  hi.Draw();
  lo.SetLineColor( kBlue );
  lo.SetLineWidth( 2 );
  lo.SetLineStyle( 3 );
  lo.Draw();

  boost::format injectfmt( "signal events %1% %2%" );
  const string inject_s
    = limnamer.CheckInput( "relmag" ) ? boost::str( injectfmt % 'x' % limnamer.GetInput<double>( "relmag" ) ) :
      boost::str( injectfmt % '=' % limnamer.GetInput<double>( "absmag" ) );

  mgr::LatexMgr latex;
  latex.SetOrigin( PLOT_X_MIN, PLOT_Y_MAX+TEXT_MARGIN/2, BOTTOM_LEFT )
  .WriteLine( limnamer.GetChannelEXT( "Root Name" ) )
  .SetOrigin( PLOT_X_TEXT_MAX, PLOT_Y_TEXT_MAX, TOP_RIGHT )
  .WriteLine( limnamer.GetExt<string>( "fitfunc", "Full Name" ) )
  .WriteLine( inject_s );

  mgr::SaveToPDF( c, limnamer.PlotFileName( "pullvmass", RhoTag(), tag,  SigStrengthTag() ) );

  delete graph;
  delete meanerr;
  delete uperr;
  delete lowerr;
  delete mg;
  delete c;
}


#include <algorithm>
#include <array>
#include <map>
#include <numeric>
#include <stdexcept>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>

#include <boost/functional/hash.hpp>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Kernel/global_functions.h>
#include <Euclid/Util/Assert.h>

namespace Euclid
{

namespace _impl
{

/** Convert face index order to a canonical form such that
 *  the first index is the smallest.*/
template<typename T, int N>
std::array<T, N> to_canonical(const std::array<T, N>& face)
{
    auto iter = std::min_element(face.begin(), face.end());
    std::array<T, N> canonical;
    for (size_t i = 0; i < N; ++i) {
        canonical[i] = *iter++;
        if (iter == face.end())
            iter = face.begin();
    }
    return canonical;
}

} // namespace _impl

template<typename T>
size_t remove_duplicate_vertices(std::vector<T>& positions)
{
    if (positions.empty()) {
        EWARNING("positions is empty.");
        return 0;
    }
    if (positions.size() % 3 != 0) {
        throw std::runtime_error("Input position size is not divisible by 3");
    }
    using Point = std::array<T, 3>;

    // Find duplicate points
    std::vector<size_t> marks;
    std::unordered_set<Point, boost::hash<Point>> unique_points;
    for (size_t i = 0; i < positions.size(); i += 3) {
        Point p{ { positions[i], positions[i + 1], positions[i + 2] } };
        auto [dummy, is_unique] = unique_points.insert(std::move(p));
        if (!is_unique) {
            marks.push_back(i);
        }
    }

    // Move elements in the back into slots to be removed
    size_t idx = positions.size() - 3;
    for (auto iter = marks.rbegin(); iter != marks.rend(); ++iter) {
        positions[*iter] = positions[idx];
        positions[*iter + 1] = positions[idx + 1];
        positions[*iter + 2] = positions[idx + 2];
        idx -= 3;
    }

    // Now drop the end
    positions.erase(positions.begin() + idx + 3, positions.end());
    positions.shrink_to_fit();

    return marks.size();
}

template<int N, typename T1, typename T2>
size_t remove_duplicate_vertices(std::vector<T1>& positions,
                                 std::vector<T2>& indices)
{
    static_assert(N >= 3);
    if (positions.empty()) {
        EWARNING("positions is empty.");
        return 0;
    }
    if (indices.empty()) {
        remove_duplicate_vertices(positions);
    }
    if (positions.size() % 3 != 0) {
        throw std::runtime_error("Input position size is not divisible by 3");
    }
    if (indices.size() % N != 0) {
        std::string err_str("Input index size is not divisible by ");
        err_str.append(std::to_string(N));
        throw(err_str);
    }
    if (*std::max_element(indices.begin(), indices.end()) >=
        static_cast<T2>(positions.size() / 3)) {
        throw std::runtime_error(
            "Input indices is out of range of the position vector");
    }
    using Point = std::array<T1, 3>;

    // marks[duplicate] = first
    // duplicate and first are indices into the position vector
    std::map<size_t, size_t> marks;

    // unique_points[point] = index
    // index refers to the position vector
    std::unordered_map<Point, size_t, boost::hash<Point>> unique_points;

    // Map all the duplicate positions to their first appearance
    for (size_t i = 0; i < positions.size(); i += 3) {
        Point p{ { positions[i], positions[i + 1], positions[i + 2] } };
        auto [iter, is_unique] = unique_points.try_emplace(std::move(p), i);
        if (!is_unique) {
            EASSERT(p == iter->first);
            marks[i] = iter->second;
        }
    }

    size_t idx = positions.size() - 3;

    // values in index_swap refers to the index of a Point
    std::vector<size_t> index_swap(positions.size() / 3);
    std::iota(index_swap.begin(), index_swap.end(), 0);

    for (auto iter = marks.rbegin(); iter != marks.rend(); ++iter, idx -= 3) {
        // Replace positions in marks with positions in the back of the vector
        auto pos = iter->first;
        positions[pos] = positions[idx];
        positions[pos + 1] = positions[idx + 1];
        positions[pos + 2] = positions[idx + 2];

        // Swap indices accordingly
        std::swap(index_swap[pos / 3], index_swap[idx / 3]);
    }

    // index_map[old] = new
    // old and new are indices of Point
    std::vector<size_t> index_map(positions.size() / 3);

    for (size_t i = 0; i < index_swap.size() - marks.size(); ++i) {
        EASSERT(marks.find(index_swap[i] * 3) == marks.end());
        index_map[index_swap[i]] = i;
    }
    for (size_t i = index_swap.size() - marks.size(); i < index_swap.size();
         ++i) {
        EASSERT(marks.find(index_swap[i] * 3) != marks.end());
        index_map[index_swap[i]] = index_map[marks[index_swap[i] * 3] / 3];
    }

    // Now fix indices
    for (auto& i : indices) {
        auto new_idx = index_map[i];
        EASSERT(new_idx < (positions.size() / 3 - marks.size()));
        i = static_cast<T2>(new_idx);
    }

    // Now erase garbage values at the tail of the vector
    positions.erase(positions.begin() + idx + 3, positions.end());
    positions.shrink_to_fit();

    return marks.size();
}

template<int N, typename T>
size_t remove_duplicate_faces(std::vector<T>& indices)
{
    static_assert(N >= 3);
    if (indices.empty()) {
        EWARNING("indices is empty.");
        return 0;
    }
    if (indices.size() % N != 0) {
        std::string err_str("Input index size is not divisible by ");
        err_str.append(std::to_string(N));
        throw(err_str);
    }
    using Face = std::array<T, N>;

    std::unordered_set<Face, boost::hash<Face>> unique_faces;
    std::vector<size_t> marks;
    for (size_t i = 0; i < indices.size(); i += N) {
        Face f;
        for (size_t j = 0; j < N; ++j) {
            f[j] = indices[i + j];
        }
        auto cf = _impl::to_canonical<T, N>(f);
        auto [iter, is_unique] = unique_faces.insert(std::move(cf));
        if (!is_unique) {
            marks.push_back(i);
        }
    }

    size_t idx = indices.size() - N;
    for (auto iter = marks.rbegin(); iter != marks.rend(); ++iter, idx -= N) {
        for (size_t i = 0; i < N; ++i) {
            indices[*iter + i] = indices[idx + i];
        }
    }

    indices.erase(indices.begin() + idx + N, indices.end());
    indices.shrink_to_fit();

    return marks.size();
}

template<int N, typename T1, typename T2>
size_t remove_unreferenced_vertices(std::vector<T1>& positions,
                                    std::vector<T2>& indices)
{
    static_assert(N >= 3);
    if (positions.empty()) {
        EWARNING("positions is empty.");
        return 0;
    }
    if (indices.empty()) {
        EWARNING("indices is empty.");
        return 0;
    }
    if (positions.size() % 3 != 0) {
        throw std::runtime_error("Input position size is not divisible by 3");
    }
    if (indices.size() % N != 0) {
        std::string err_str("Input index size is not divisible by ");
        err_str.append(std::to_string(N));
        throw(err_str);
    }
    if (*std::max_element(indices.begin(), indices.end()) >=
        static_cast<T2>(positions.size() / 3)) {
        throw std::runtime_error(
            "Input indices is out of range of the position vector");
    }

    std::vector<int> ref_count(positions.size() / 3, 0);
    for (auto i : indices) {
        ++ref_count[i];
    }

    size_t idx = ref_count.size() - 1;
    std::vector<T2> index_swap(ref_count.size());
    std::iota(index_swap.begin(), index_swap.end(), 0);
    for (int i = static_cast<int>(ref_count.size()) - 1; i >= 0; --i) {
        if (ref_count[i] == 0) {
            for (auto j = 0; j < 3; ++j) {
                positions[i * 3 + j] = positions[idx * 3 + j];
            }
            std::swap(index_swap[i], index_swap[idx--]);
        }
    }

    std::unordered_map<T2, size_t> index_map;
    for (size_t i = 0; i <= idx; ++i) {
        index_map[index_swap[i]] = i;
    }

    for (auto& i : indices) {
        i = static_cast<T2>(index_map[i]);
    }

    positions.erase(positions.begin() + (idx + 1) * 3, positions.end());
    positions.shrink_to_fit();

    return ref_count.size() - idx - 1;
}

template<int N, typename T1, typename T2>
size_t remove_degenerate_faces(const std::vector<T1>& positions,
                               std::vector<T2>& indices)
{
    static_assert(N >= 3);
    if (positions.empty()) {
        EWARNING("position is empty.");
        return 0;
    }
    if (indices.empty()) {
        EWARNING("indices is empty.");
        return 0;
    }
    if (positions.size() % 3 != 0) {
        throw std::runtime_error("Input position size is not divisible by 3");
    }
    if (indices.size() % N != 0) {
        std::string err_str("Input index size is not divisible by ");
        err_str.append(std::to_string(N));
        throw(err_str);
    }
    if (*std::max_element(indices.begin(), indices.end()) >=
        static_cast<T2>(positions.size() / 3)) {
        throw std::runtime_error(
            "Input indices is out of range of the position vector");
    }
    using Kernel = CGAL::Simple_cartesian<T1>;
    using Point_3 = typename Kernel::Point_3;

    std::vector<size_t> marks;
    for (size_t i = 0; i < indices.size(); i += N) {
        for (size_t j = 0; j < N - 1; ++j) {
            auto p0 = indices[i + j] * 3;
            auto p1 = indices[i + j + 1] * 3;
            auto p2 = (j == N - 2 ? indices[i] : indices[i + j + 2]) * 3;
            auto x0 = positions[p0];
            auto y0 = positions[p0 + 1];
            auto z0 = positions[p0 + 2];
            auto x1 = positions[p1];
            auto y1 = positions[p1 + 1];
            auto z1 = positions[p1 + 2];
            auto x2 = positions[p2];
            auto y2 = positions[p2 + 1];
            auto z2 = positions[p2 + 2];
            if (CGAL::collinear<Kernel>(Point_3{ x0, y0, z0 },
                                        Point_3{ x1, y1, z1 },
                                        Point_3{ x2, y2, z2 })) {
                marks.push_back(i);
                break;
            }
        }
    }

    auto idx = indices.size() - N;
    for (auto iter = marks.rbegin(); iter != marks.rend(); ++iter, idx -= N) {
        for (auto i = 0; i < N; ++i) {
            indices[*iter + i] = indices[idx + i];
        }
    }

    indices.erase(indices.begin() + idx + N, indices.end());
    indices.shrink_to_fit();

    return marks.size();
}

} // namespace Euclid