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

/* Test_ThinEvalMap.cpp - Testing the evalution map for thin bootstrapping
 */
#include <cassert>
#include <helib/helib.h>
#include <helib/EvalMap.h>
#include <NTL/BasicThreadPool.h>
#include <helib/ArgMap.h>

NTL_CLIENT
using namespace helib;

static bool dry = false; // a dry-run flag
static bool noPrint = true;

void  TestIt(long p, long r, long c, long _k, long w,
             long L, Vec<long>& mvec,
             Vec<long>& gens, Vec<long>& ords, long useCache)
{
  if (lsize(mvec)<1) { // use default values
    mvec.SetLength(3); gens.SetLength(3); ords.SetLength(3);
    mvec[0] = 7;    mvec[1] = 3;    mvec[2] = 221;
    gens[0] = 3979; gens[1] = 3095; gens[2] = 3760;
    ords[0] = 6;    ords[1] = 2;    ords[2] = -8;
  }
  if (!noPrint)
    cout << "*** TestIt"
       << (dry? " (dry run):" : ":")
       << " p=" << p
       << ", r=" << r
       << ", c=" << c
       << ", k=" << _k
       << ", w=" << w
       << ", L=" << L
       << ", mvec=" << mvec << ", "
       << ", useCache = " << useCache
       << endl;

  setTimersOn();
  setDryRun(false); // Need to get a "real context" to test ThinEvalMap

  // mvec is supposed to include the prime-power factorization of m
  long nfactors = mvec.length();
  for (long i = 0; i < nfactors; i++)
    for (long j = i+1; j < nfactors; j++)
      assert(GCD(mvec[i], mvec[j]) == 1);

  // multiply all the prime powers to get m itself
  long m = computeProd(mvec);
  assert(GCD(p, m) == 1);

  // build a context with these generators and orders
  vector<long> gens1, ords1;
  convert(gens1, gens);
  convert(ords1, ords);
  Context context(m, p, r, gens1, ords1);
  buildModChain(context, L, c);

  if (!noPrint) {
    context.zMStar.printout(); // print structure of Zm* /(p) to cout
    cout << endl;
  }
  long d = context.zMStar.getOrdP();
  long phim = context.zMStar.getPhiM();
  long nslots = phim/d;

  setDryRun(dry); // Now we can set the dry-run flag if desired

  SecKey secretKey(context);
  const PubKey& publicKey = secretKey;
  secretKey.GenSecKey(w); // A Hamming-weight-w secret key
  addSome1DMatrices(secretKey); // compute key-switching matrices that we need
  addFrbMatrices(secretKey); // compute key-switching matrices that we need

  // GG defines the plaintext space Z_p[X]/GG(X)
  ZZX GG;
  GG = context.alMod.getFactorsOverZZ()[0];
  EncryptedArray ea(context, GG);

  zz_p::init(context.alMod.getPPowR());

  Vec<zz_p> val0(INIT_SIZE, nslots);
  for (auto& x: val0)
    random(x);

  vector<ZZX> val1;
  val1.resize(nslots);
  for (long i = 0; i < nslots; i++) {
    val1[i] = conv<ZZX>(conv<ZZ>(rep(val0[i])));
  }

  Ctxt ctxt(publicKey);
  ea.encrypt(ctxt, publicKey, val1);

  resetAllTimers();
  FHE_NTIMER_START(ALL);

  // Compute homomorphically the transformation that takes the
  // coefficients packed in the slots and produces the polynomial
  // corresponding to cube

  if (!noPrint) CheckCtxt(ctxt, "init");

  if (!noPrint) cout << "build ThinEvalMap\n";
  ThinEvalMap map(ea, /*minimal=*/false, mvec,
    /*invert=*/false, /*build_cache=*/false);
  // compute the transformation to apply

  if (!noPrint) cout << "apply ThinEvalMap\n";
  if (useCache) map.upgrade();
  map.apply(ctxt); // apply the transformation to ctxt
  if (!noPrint) CheckCtxt(ctxt, "ThinEvalMap");
  if (!noPrint) cout << "check results\n";

  if (!noPrint) cout << "build ThinEvalMap\n";
  ThinEvalMap imap(ea, /*minimal=*/false, mvec,
    /*invert=*/true, /*build_cache=*/false);
  // compute the transformation to apply
  if (!noPrint) cout << "apply ThinEvalMap\n";
  if (useCache) imap.upgrade();
  imap.apply(ctxt); // apply the transformation to ctxt
  if (!noPrint) {
    CheckCtxt(ctxt, "ThinEvalMap");
    cout << "check results\n";
  }

#if 1

  /* create dirty version of ctxt */
  Vec<zz_pX> dirty_val0;
  dirty_val0.SetLength(nslots);
  for (long i = 0; i < nslots; i++) {
    random(dirty_val0[i], d);
    SetCoeff(dirty_val0[i], 0, val0[i]);
  }

  vector<ZZX> dirty_val1;
  dirty_val1.resize(nslots);
  for (long i = 0; i < nslots; i++) {
    dirty_val1[i] = conv<ZZX>(dirty_val0[i]);
  }

  Ctxt dirty_ctxt(publicKey);
  ea.encrypt(dirty_ctxt, publicKey, dirty_val1);

  EvalMap dirty_map(ea, /*minimal=*/false, mvec,
    /*invert=*/false, /*build_cache=*/false);

  dirty_map.apply(dirty_ctxt);
  imap.apply(dirty_ctxt);
#endif

  vector<ZZX> val2;
  ea.decrypt(ctxt, secretKey, val2);
  cout << ((val1 == val2)? "GOOD\n" : "BAD\n");

  vector<ZZX> dirty_val2;
  ea.decrypt(dirty_ctxt, secretKey, dirty_val2);
  cout << ((val1 == dirty_val2)? "GOOD\n" : "BAD\n");

  FHE_NTIMER_STOP(ALL);

  if (!noPrint) {
    cout << "\n*********\n";
    printAllTimers();
    cout << endl;
  }
}

/* Usage: Test_EvalMap_x.exe [ name=value ]...
 *  p       plaintext base  [ default=2 ]
 *  r       lifting  [ default=1 ]
 *  c       number of columns in the key-switching matrices  [ default=2 ]
 *  k       security parameter  [ default=80 ]
 *  L       # of bits in the modulus chain
 *  s       minimum number of slots  [ default=0 ]
 *  seed    PRG seed  [ default=0 ]
 *  mvec    use specified factorization of m
 *             e.g., mvec='[5 3 187]'
 *  gens    use specified vector of generators
 *             e.g., gens='[562 1871 751]'
 *  ords    use specified vector of orders
 *             e.g., ords='[4 2 -4]', negative means 'bad'
 */
int main(int argc, char *argv[])
{
  ArgMap amap;

  long p=2;
  amap.arg("p", p, "plaintext base");

  long r=1;
  amap.arg("r", r,  "lifting");

  long c=2;
  amap.arg("c", c, "number of columns in the key-switching matrices");

  long k=80;
  amap.arg("k", k, "security parameter");

  long L=300;
  amap.arg("L", L, "# of levels in the modulus chain");

  long s=0;
  amap.arg("s", s, "minimum number of slots");

  long seed=0;
  amap.arg("seed", seed, "PRG seed");

  Vec<long> mvec;
  amap.arg("mvec", mvec, "use specified factorization of m", nullptr);
  amap.note("e.g., mvec='[7 3 221]'");

  Vec<long> gens;
  amap.arg("gens", gens, "use specified vector of generators", nullptr);
  amap.note("e.g., gens='[3979 3095 3760]'");

  Vec<long> ords;
  amap.arg("ords", ords, "use specified vector of orders", nullptr);
  amap.note("e.g., ords='[6 2 -8]', negative means 'bad'");

  amap.arg("dry", dry, "a dry-run flag to check the noise");

  long nthreads=1;
  amap.arg("nthreads", nthreads, "number of threads");

  amap.arg("noPrint", noPrint, "suppress printouts");

  long useCache=0;
  amap.arg("useCache", useCache, "0: zzX cache, 2: DCRT cache");

  amap.parse(argc, argv);

  SetNumThreads(nthreads);

  SetSeed(conv<ZZ>(seed));
  TestIt(p, r, c, k, /*Key Hamming weight=*/64, L, mvec, gens, ords, useCache);
}

// ./Test_ThinEvalMap_x mvec="[73 433]" gens="[18620 12995]" ords="[72 -6]"


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/arithmetic/include/functions/iround.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/three.hpp>
#include <boost/simd/include/constants/two.hpp>

NT2_TEST_CASE_TPL ( iround_real__1_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::iround;
  using boost::simd::tag::iround_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<iround_(vT)>::type r_t;

  // specific values tests
  NT2_TEST_ULP_EQUAL(iround(boost::simd::splat<vT>(1.4)), boost::simd::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround(boost::simd::splat<vT>(1.5)), boost::simd::Two<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround(boost::simd::splat<vT>(1.6)), boost::simd::Two<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround(boost::simd::splat<vT>(2.5)), boost::simd::Three<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround(boost::simd::Half<vT>()), boost::simd::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround(boost::simd::Inf<vT>()), boost::simd::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround(boost::simd::Mhalf<vT>()), boost::simd::Mone<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround(boost::simd::Minf<vT>()), boost::simd::Minf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround(boost::simd::Mone<vT>()), boost::simd::Mone<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround(boost::simd::Nan<vT>()), boost::simd::Zero<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround(boost::simd::One<vT>()), boost::simd::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround(boost::simd::Zero<vT>()), boost::simd::Zero<r_t>(), 0);
} // end of test for floating_


/*
 ******************************************************************
 *           Delimiter Seperated Values Filter Library            *
 *                                                                *
 * Author: Arash Partow (2004)                                    *
 * URL: http://www.partow.net/programming/dsvfilter/index.html    *
 *                                                                *
 * Copyright notice:                                              *
 * Free use of the Delimiter Seperated Values Filter Library is   *
 * permitted under the guidelines and in accordance with the most *
 * current version of the Common Public License.                  *
 * http://www.opensource.org/licenses/cpl1.0.php                  *
 *                                                                *
 ******************************************************************
*/

#ifndef INCLUDE_DSV_FILTER_HPP
#define INCLUDE_DSV_FILTER_HPP

#include <string>
#include <deque>
#include <vector>

#define strtk_no_tr1_or_boost

#include "exprtk.hpp"
#include "strtk.hpp"

#ifdef dsv_filter_use_mmap
  #include <boost/iostreams/device/mapped_file.hpp>
#endif

class dsv_filter
{
public:

   struct column_properties
   {
      enum column_type
      {
         e_none,
         e_string,
         e_number
      };

      column_properties()
      : type(e_none),
        name(""),
        value_s(""),
        value_n(0.0),
        process(false)
      {}

      column_type type;
      std::string name;
      std::string value_s;
      double value_n;
      strtk::util::value value;
      bool process;
   };

   dsv_filter()
   : file_name_(""),
     input_delimiter_(","),
     output_delimiter_("|")
   {
      symbol_table_.add_constants();
      expression_.register_symbol_table(symbol_table_);
   }

   inline std::string file_name() const
   {
      return file_name_;
   }

   inline void set_input_delimiter(const std::string& input_delimiter)
   {
      input_delimiter_ = input_delimiter;
   }

   inline void set_output_delimiter(const std::string& output_delimiter)
   {
      output_delimiter_ = output_delimiter;
   }

   inline std::string input_delimiter() const
   {
      return input_delimiter_;
   }

   inline std::string output_delimiter() const
   {
      return output_delimiter_;
   }

   inline std::size_t column_count() const
   {
      return column_.size();
   }

   inline std::size_t row_count() const
   {
      return grid_.row_count();
   }

   inline const column_properties& column(const std::size_t& index) const
   {
      return column_[index];
   }

   inline bool load(const std::string& file_name)
   {
      if (!strtk::fileio::file_exists(file_name))
         return false;
      file_name_ = file_name;
      strtk::token_grid::options options;
      options.column_delimiters = input_delimiter_;
      #ifdef dsv_filter_use_mmap
         input_source.close();
         input_source.open(file_name_);
         unsigned char* data = reinterpret_cast<unsigned char*>(const_cast<char*>(input_source.data()));
         if (!grid_.load(data,input_source.size(),options))
            return false;
      #else
         if (!grid_.load(file_name_,options))
            return false;
      #endif
      if (0 == grid_.row_count())
         return false;
      if (grid_.row_count() < 2)
         return false;
      if(!process_column_header())
         return false;
      return true;
   }

   inline bool add_filter(const std::string& filter_expression)
   {
      error_ = "";
      parser_.cache_symbols() = true;
      if (!parser_.compile(filter_expression,expression_))
      {
         error_ = "Error: " + parser_.error() + "\tFilter: " + filter_expression;
         return false;
      }

      // Only extract for processing, the column values for that are
      // being actively used in the current expression.
      std::deque<std::string> symbol;
      parser_.expression_symbols(symbol);
      for (std::size_t i = 0; i < column_.size(); ++i)
      {
         if (column_[i].name.empty())
            continue;
         column_[i].process = false;
         for (std::size_t j = 0; j < symbol.size(); ++j)
         {
            if (strtk::imatch(symbol[j],column_[i].name))
            {
               column_[i].process = true;
               break;
            }
         }
      }
      return true;
   }

   template<typename Allocator,
            template <typename,typename> class Sequence>
   inline bool row(const std::size_t& r,
                   const Sequence<bool,Allocator>& selected_column,
                   std::string& row_result)
   {
      if (selected_column.size() != column_.size())
      {
         error_ = "Error: number of selected columns larger than number of columns";
         return false;
      }
      if (r >= grid_.row_count())
      {
         strtk::build_string s;
         s << "Error: row[" << r << "] out of bounds.";
         error_ = s.as_string();
         return false;
      }
      if (row_.index() != r)
      {
         row_ = grid_.row(r);
      }
      for (std::size_t c = 0; c < column_.size(); ++c)
      {
         if (selected_column[c])
         {
            strtk::token_grid::range_t token = row_.token(c);
            row_result.append(token.first,token.second);
            if (c < (column_.size() - 1))
            {
               row_result.append(output_delimiter_);
            }
         }
      }
      if (!row_result.empty() && row_result[row_result.size() - 1])
      {
         row_result.resize(row_result.size() - 1);
      }
      return true;
   }

   inline std::string error()
   {
      return error_;
   }

   enum filter_result
   {
      e_error,
      e_match,
      e_mismatch
   };

   inline filter_result operator[](const std::size_t& r)
   {
      row_ = grid_.row(r);
      for (std::size_t c = 0; c < column_.size(); ++c)
      {
         if (!column_[c].process)
            continue;
         else if (!row_.parse_with_index(c,column_[c].value))
         {
            strtk::build_string s;
            s << "Error: Failed to process element at row/col["<< r << "," << c << "]  value:" << row_.get<std::string>(c);
            error_ = s.as_string();
            return e_error;
         }
      }
      return (1.0 == expression_.value()) ? e_match : e_mismatch;
   }

   const strtk::token_grid& grid() const
   {
      return grid_;
   }

private:

   inline bool process_column_header()
   {
      static const std::string string_id ("_s");
      static const std::string number_id ("_n");
      expression_.get_symbol_table().clear();
      column_.clear();
      column_.resize(grid_.row(0).size());
      strtk::token_grid::row_type row = grid_.row(0);
      std::string col_name = "";
      std::string col_suffix = "";
      for (std::size_t i = 0; i < row.size(); ++i)
      {
         column_properties& column = column_[i];
         column.process = false;
         col_name = row.get<std::string>(i);
         col_suffix = (col_name.size() >= 2) ? strtk::text::remaining_string(col_name.size() - 2,col_name) : "";
         col_name = col_name.substr(0,col_name.size() - 2);
         if (symbol_table_.symbol_exists(col_name))
         {
            error_ = "Error: Redefinition of column " + col_name;
            return false;
         }
         else if (strtk::ends_with("_s",col_suffix) || strtk::ends_with("_S",col_suffix))
         {
            column.type    = dsv_filter::column_properties::e_string;
            column.name    = col_name;
            column.value   = strtk::util::value(column.value_s);
            column.process = true;
            symbol_table_.add_stringvar(col_name,column.value_s);
         }
         else if (strtk::ends_with("_n",col_suffix) || strtk::ends_with("_N",col_suffix))
         {
            column.type    = dsv_filter::column_properties::e_number;
            column.name    = col_name;
            column.process = true;
            column.value = strtk::util::value(column.value_n);
            symbol_table_.add_variable(col_name,column.value_n);
         }
      }
      return true;
   }

   std::string file_name_;
   std::string input_delimiter_;
   std::string output_delimiter_;
   std::string error_;
   std::vector<column_properties> column_;
   strtk::token_grid grid_;
   exprtk::symbol_table<double> symbol_table_;
   exprtk::parser<double> parser_;
   exprtk::expression<double> expression_;
   strtk::token_grid::row_type row_;

   #ifdef dsv_filter_use_mmap
   boost::iostreams::mapped_file_source input_source;
   #endif

};

#endif


#include <algorithm>
#include <array>
#include <iostream>
#include <numeric>
#include <utility>
#include <vector>

#include <boost/range/irange.hpp>

#include "input.hpp"

using intmax = std::intmax_t;
using uintmax = std::uintmax_t;

// compile-time parsing so I can use arrays later on without magic numbers
struct ParseStats {
  uintmax num_words,
          num_lines;
};

constexpr auto SPACE = ' ';
constexpr auto NEWLINE = '\n';

constexpr auto parse_stats(std::string_view input) {
  auto stats = ParseStats{};

  auto& num_words = stats.num_words;
  auto& num_lines = stats.num_lines;

  for(auto c : input) {
    num_words += (c == SPACE);
    num_lines += (c == NEWLINE);
  }

  // there's no newline character on the last line, so we need to add it manually
  ++num_lines;
  // splitting by whitespace misses the last word, so we need to add the rest manually, too
  num_words += num_lines;

  return stats;
}

constexpr auto PARSE_STATS = parse_stats(puzzle_input);
constexpr auto NUM_WORDS = PARSE_STATS.num_words;
constexpr auto NUM_LINES = PARSE_STATS.num_lines;

constexpr auto N = 4;
constexpr auto M = 5;

using IntArray = std::array<intmax, M>;

using Ingredient = IntArray;
using Properties = IntArray;

using Ingredients = std::array<IntArray, N>;

auto split_input(std::string_view input) {
  auto lines = std::array<decltype(input), NUM_LINES>{};
  decltype(input.size()) pos = 0;
  auto index = 0;
  while(pos != input.npos) {
    pos = input.find(NEWLINE);
    lines[index++] = input.substr(0, pos);
    input.remove_prefix(pos + 1);
  }
  return lines;
}

auto split_line(std::string_view line) {
  auto words = std::array<decltype(line), NUM_WORDS>{};
  decltype(line.size()) pos = 0;
  auto index = 0;
  while(pos != line.npos) {
    pos = line.find(SPACE);
    words[index++] = line.substr(0, pos);
    line.remove_prefix(pos + 1);
  }
  return words;
}

auto to_int(std::string_view input) {
  auto result = intmax{};
  const auto negative = (input.front() == '-');
  if(negative) {
    input.remove_prefix(1);
  }
  for(auto c : input) {
    result *= 10;
    result += (c - '0');
  }
  return (negative ? -result : result);
}

auto parse(std::string_view input) {

  const auto to_int_suffix = [] (auto token) {
    token.remove_suffix(1);
    return to_int(token);
  };

  auto ingredients = Ingredients{};
  auto index = 0;

  for(const auto& line : split_input(input)) {

    const auto& words = split_line(line);

    const auto word = [&words] (auto i) {
      return words[(i + 1) * 2];
    };

    auto& ingredient = ingredients[index++];

    for(const auto i : boost::irange(N)) {
      ingredient[i] = to_int_suffix(word(i));
    }

    ingredient[N] = to_int(word(N));
  }

  return ingredients;
}

auto plus(const Properties& lhs, const Properties& rhs) {
  auto result = Properties{};
  // element-wise addition of lhs and rhs
  std::transform(lhs.begin(), lhs.end(), rhs.begin(), result.begin(), std::plus{});
  return result;
}

auto multiply(const intmax scalar, const Ingredient& ingredient) {
  auto result = Properties{};
  std::transform(ingredient.begin(), ingredient.end(), result.begin(), [scalar] (auto property) {
    return (scalar * property);
  });
  return result;
}

auto get_max_score(const Ingredients& ingredients) {

  auto max_score = intmax{};

  intmax min_spoons = 1,
         max_spoons = 100;

  const intmax calories = 500;

  for(auto fr = min_spoons; fr < (max_spoons - 2); ++fr) {

    for(auto ca = min_spoons; ca < (max_spoons - fr - 1); ++ca) {

      for(auto bu = min_spoons; bu < (max_spoons - fr - ca); ++bu) {

        const auto su = (max_spoons - fr - ca - bu);

        const auto recipe = IntArray{fr, ca, bu, su};

        const auto properties = std::inner_product(recipe.begin(), recipe.end(), ingredients.begin(), Properties{}, plus, multiply);

        if(properties.back() == calories) {

          const auto new_score = std::accumulate(properties.begin(), (properties.end() - 1), intmax{1}, [] (auto acc, auto value) {
            return acc * ((value > 0) ? value : 0);
          });

          max_score = std::max(new_score, max_score);

        }
      }
    }
  }

  return max_score;
}

auto solution(std::string_view input) {

  const auto ingredients = parse(input);

  return get_max_score(ingredients);
}

int main() {

  std::cout << solution(puzzle_input) << std::endl;

}


#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
//#include <opencv2/features2d.hpp>
#include <iostream>
#include <armadillo>
#include "highgui.h"
#include "imgproc.h"
#include "videotest.h"

using namespace std;
using namespace cv;

void detectBear(cv::Mat &camframe, std::mutex &camlock, arma::vec & bearpos, double &bearsize, bool &bearfound) {
  const int h1 = 0;
  const int h2 = 100;
  const int s1 = 128;
  const int s2 = 255;
  const int v1 = 128;
  const int v2 = 255;
	arma::vec xbuf(10, arma::fill::zeros);
	arma::vec ybuf(10, arma::fill::zeros);
	arma::vec xcbuf(10, arma::fill::zeros);
	arma::vec ycbuf(10, arma::fill::zeros);
	int xind = 0, yind = 0;
	bool stable = false;
	VideoCapture cam(1);
	Mat img;
	while (1) {
	  cam.read(img);
	  //if (!img.data) {
	//	continue;
	 // }
		camlock.lock();
		img.copyTo(camframe);
		camlock.unlock();
		Mat hsv;
		cvtColor(img, hsv, COLOR_BGR2HSV);

		// in range masking
		Mat hsv2mask, hsv2;
		inRange(hsv, Scalar(h1, s1, v1), Scalar(h2, s2, v2), hsv2mask);
		//hsv.copyTo(hsv2, hsv2mask);

		arma::mat I;
		cvt_opencv2arma(hsv2mask, I);

		//blur
		//I = conv2(I, gauss2(7, 16));

		arma::rowvec r = arma::sum(I, 0);
		arma::colvec c = arma::sum(I, 1);
		double midx = arma::sum(r * arma::cumsum(arma::ones<arma::vec>(r.n_elem))) / arma::sum(r);
		double midy = arma::sum(c % arma::cumsum(arma::ones<arma::vec>(c.n_elem))) / arma::sum(c);
		stable = true;
		if (midx < 1 || (midx > (int)I.n_cols - 1) || arma::sum(r) == 0) {
			midx = 0;
		}
		if (midy < 1 || (midy > (int)I.n_rows - 1) || arma::sum(c) == 0) {
			midy = 0;
		}

		// remove noise
		if (midx == 0 || midy == 0) {
			stable = false;
		}
		xbuf[xind] = midx;
		ybuf[yind] = midy;
		midx = arma::mean(xbuf);
		midy = arma::mean(ybuf);
		if (sqrt(arma::var(xbuf) * arma::var(xbuf) + arma::var(ybuf) + arma::var(ybuf)) > 70.0) {
			stable = false;
		}

		Scalar color(255, 0, 0);
		//cout << "arma:: " << midx << ", " << midy << endl;
		//cout << "stable: " << stable << endl;
		circle(img, Point((int)midx, (int)midy), 4, color, 0);

		double covx = sqrt(arma::sum(r * arma::square(arma::cumsum(arma::ones<arma::vec>(r.n_elem)) - midx)) / arma::sum(r));
		double covy = sqrt(arma::sum(c % arma::square(arma::cumsum(arma::ones<arma::vec>(c.n_elem)) - midy)) / arma::sum(c));
		xcbuf[xind] = covx;
		ycbuf[yind] = covy;
		covx = arma::accu(xcbuf) / (int)xbuf.n_elem;
		covy = arma::accu(ycbuf) / (int)ybuf.n_elem;
		xind = (xind + 1) % (int)xbuf.n_elem;
		yind = (yind + 1) % (int)ybuf.n_elem;
		//double estw = covx * 3;
		//double esth = covy * 3;
		//blur the image to make it more accurate
		/*I = conv2(I, gauss2(7, 16));
		disp_image("convI", I);
		int minw = (int)midx;
		int maxw = (int)midx;
		for (int minw = (int)midx; minw >= 0; minw--) {
		  if (I(midy, minw) < 0.05) {
			break;
		  }
		}
		for (int maxw = (int)midx; maxw < 640; maxw++) {
		  if (I(midy, maxw) < 0.05) {
			break;
		  }
		}
		int minh = (int)midy;
		int maxh = (int)midy;
		for (int minh = (int)midy; minh >= 0; minh--) {
		  if (I(minh, midx) < 0.05) {
			break;
		  }
		}
		for (int maxh = (int)midy; maxh < 480; maxh++) {
		  if (I(maxh, midx) < 0.05) {
			break;
		  }
		}
		double covy = (double)(maxh - minh);
		double covx = (double)(maxw - minw);*/
		covx = (covx) * 3.3;
		covy = (covy) * 3.3;
		bearpos = arma::vec({ midx - 320, 479 - (midy - 240) });
		bearsize = 533 / (covx) * 12;
		bearfound = stable;

		rectangle(img, Rect(midx-covx/2,midy-covy/2,covx,covy), Scalar(0, 0, 255), 2);

		imshow("oldhsv", hsv);
		imshow("newhsv", hsv2mask);
		imshow("img", img);
		waitKey(30);
	}
}
#if 0

int main(int argc, char *argv[]) {
	if (argc != 8) {
		cout << "usage: ./test img h1 h2 s1 s2 v1 v2\n";
		return 1;
	}
	int h1 = atoi(argv[2]);
	int h2 = atoi(argv[3]);
	int s1 = atoi(argv[4]);
	int s2 = atoi(argv[5]);
	int v1 = atoi(argv[6]);
	int v2 = atoi(argv[7]);

	// convert to hsv
	VideoCapture cam(0);

	arma::vec xbuf(10, arma::fill::zeros);
	arma::vec ybuf(10, arma::fill::zeros);
	int xind = 0, yind = 0;
	bool stable = false;

	while (1) {
		Mat img;
		cam.read(img);
		Mat hsv;
		cvtColor(img, hsv, COLOR_BGR2HSV);

		// in range masking
		Mat hsv2mask, hsv2;
		inRange(hsv, Scalar(h1, s1, v1), Scalar(h2, s2, v2), hsv2mask);
		//hsv.copyTo(hsv2, hsv2mask);

		arma::mat I;
		cvt_opencv2arma(hsv2mask, I);
		arma::rowvec r = arma::sum(I, 0);
		arma::colvec c = arma::sum(I, 1);
		double midx = arma::sum(r * arma::cumsum(arma::ones<arma::vec>(r.n_elem))) / arma::sum(r);
		double midy = arma::sum(c % arma::cumsum(arma::ones<arma::vec>(c.n_elem))) / arma::sum(c);
		stable = true;
		if (midx < 1 || (midx > (int)I.n_cols - 1) || arma::sum(r) == 0) {
			midx = 0;
		}
		if (midy < 1 || (midy > (int)I.n_rows - 1) || arma::sum(c) == 0) {
			midy = 0;
		}

		// remove noise
		if (midx == 0 || midy == 0) {
			stable = false;
		}
		xbuf[xind] = midx;
		ybuf[yind] = midy;
		xind = (xind + 1) % (int)xbuf.n_elem;
		yind = (yind + 1) % (int)ybuf.n_elem;
		midx = arma::mean(xbuf);
		midy = arma::mean(ybuf);
		if (sqrt(arma::var(xbuf) * arma::var(xbuf) + arma::var(ybuf) + arma::var(ybuf)) > 70.0) {
			stable = false;
		}

		Scalar color(255, 0, 0);
		cout << "arma:: " << midx << ", " << midy << endl;
		cout << "stable: " << stable << endl;
		circle(img, Point((int)midx, (int)midy), 4, color, 0);

		double covx = sqrt(arma::sum(r * arma::square(arma::cumsum(arma::ones<arma::vec>(r.n_elem)) - midx)) / arma::sum(r));
		double covy = sqrt(arma::sum(c % arma::square(arma::cumsum(arma::ones<arma::vec>(c.n_elem)) - midy)) / arma::sum(c));
		double estw = covx * 3;
		double esth = covy * 3;

		rectangle(img, Rect(midx-estw/2,midy-esth/2,estw,esth), Scalar(0, 0, 255), 2);

		// use histogram binning to get the center

		// create blob params
		/*SimpleBlobDetector::Params params;

		// Change thresholds
		params.minThreshold = 10;
		params.maxThreshold = 200;

		// Filter by Area.
		params.filterByArea = true;
		params.minArea = 1500;

		// Filter by Circularity
		params.filterByCircularity = true;
		params.minCircularity = 0.1;

		// Filter by Convexity
		params.filterByConvexity = true;
		params.minConvexity = 0.87;

		// Filter by Inertia
		params.filterByInertia = true;
		params.minInertiaRatio = 0.01;

		Ptr<SimpleBlobDetector> blob = SimpleBlobDetector::create(params);
		vector<KeyPoint> kp;
		Mat des;
		Mat hsvd = hsv2mask * -0.75 + 255;
		blob->detect(hsvd, kp);

		cout << "found " << kp.size() << " matches\n";

		Mat kpimg;
		drawKeypoints(img, kp, kpimg, Scalar(0, 0, 255), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);*/

		imshow("oldhsv", hsv);
		imshow("newhsv", hsv2mask);
		imshow("img", img);
		if (waitKey(30) & 0xff == 'Q') {
			break;
		}
	}
	return 1;
}
#endif


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

#include "theia/sfm/camera/radial_distortion.h"

#include <Eigen/Core>
#include <glog/logging.h>

#include <algorithm>
#include <cmath>
#include <limits>

#include "theia/math/polynomial.h"

namespace theia {

// Solves for the undistorted image points. We assume that the radial distortion
// model is:
//   r = x * x + y * y;
//   d = 1 + r * (k1 + k2 * r);
//   xp = x * d;
//   yp = y * d;
//
// Given this model, we know that:
//   xp / x  =  yp / y
// So we can rewrite r such that
//   r = (1 + yp * yp / (yp * xp)) * x * x
// and now we have r in terms of a single unknown x. Plugging this into
// xp = x * d we have a 5th order polynomial in x that we can solve with an
// iterative solver.
void RadialUndistortPoint(const Eigen::Vector2d& distorted_point,
                          const double radial_distortion1,
                          const double radial_distortion2,
                          Eigen::Vector2d* undistorted_point) {
  const double kMinRadius = 1e-5;
  const double kEpsilon = 1e-8;
  const int kMaxIter = 10;

  if (std::max(std::abs(distorted_point.x()), std::abs(distorted_point.y())) <
      kMinRadius) {
    *undistorted_point = distorted_point;
    return;
  }

  // Choose which variable we solve around to improve stability.
  if (std::abs(distorted_point.x()) > std::abs(distorted_point.y())) {
    const double point_ratio = distorted_point.y() / distorted_point.x();
    const double r_sq = 1 + point_ratio * point_ratio;
    Eigen::VectorXd quintic_polynomial(6);
    quintic_polynomial(0) = radial_distortion2 * r_sq * r_sq;
    quintic_polynomial(1) = 0;
    quintic_polynomial(2) = radial_distortion1 * r_sq;
    quintic_polynomial(3) = 0;
    quintic_polynomial(4) = 1;
    quintic_polynomial(5) = -distorted_point.x();

    undistorted_point->x() = FindRootIterativeLaguerre(
        quintic_polynomial, distorted_point.x(), kEpsilon, kMaxIter);
    undistorted_point->y() = point_ratio * undistorted_point->x();
  } else {
    const double point_ratio = distorted_point.x() / distorted_point.y();
    const double r_sq = 1 + point_ratio * point_ratio;
    Eigen::VectorXd quintic_polynomial(6);
    quintic_polynomial(0) = radial_distortion2 * r_sq * r_sq;
    quintic_polynomial(1) = 0;
    quintic_polynomial(2) = radial_distortion1 * r_sq;
    quintic_polynomial(3) = 0;
    quintic_polynomial(4) = 1;
    quintic_polynomial(5) = -distorted_point.y();

    undistorted_point->y() = FindRootIterativeLaguerre(
        quintic_polynomial, distorted_point.y(), kEpsilon, kMaxIter);
    undistorted_point->x() = point_ratio * undistorted_point->y();
  }
}

}  // namespace theia


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

#include <boost/hana/ext/std/ratio.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/integral_domain.hpp>

#include <ratio>
using namespace boost::hana;

int main() {
    auto ratios = make<Tuple>(
          std::ratio<0>{}
        , std::ratio<1, 3>{}
        , std::ratio<1, 2>{}
        , std::ratio<2, 6>{}
        , std::ratio<3, 1>{}
        , std::ratio<7, 8>{}
        , std::ratio<3, 5>{}
        , std::ratio<2, 1>{}
    );

    //////////////////////////////////////////////////////////////////////////
    // IntegralDomain
    //////////////////////////////////////////////////////////////////////////
    {
        // quot
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                quot(std::ratio<6>{}, std::ratio<4>{}),
                std::ratio<6, 4>{}
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                quot(std::ratio<3, 4>{}, std::ratio<5, 10>{}),
                std::ratio<3*10, 4*5>{}
            ));
        }

        // rem
        {
            BOOST_HANA_CONSTANT_CHECK(equal(
                rem(std::ratio<6>{}, std::ratio<4>{}),
                std::ratio<0>{}
            ));

            BOOST_HANA_CONSTANT_CHECK(equal(
                rem(std::ratio<3, 4>{}, std::ratio<5, 10>{}),
                std::ratio<0>{}
            ));
        }

        // laws
        test::TestIntegralDomain<ext::std::Ratio>{ratios};
    }
}


#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/boost/graph/split_graph_into_polylines.h>

#include <boost/graph/adjacency_list.hpp>

#include <map>
#include <vector>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef K::Point_2 Point_2;

typedef boost::adjacency_list<boost::vecS, boost::setS, boost::undirectedS, Point_2 > Graph;

typedef boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;

typedef std::map<Point_2, vertex_descriptor> Point_vertex_map;

typedef std::vector<Point_2> Polyline_2;

// inserts a polyline into a graph
void insert(const std::vector<Point_2>& poly, Graph& graph, Point_vertex_map& pvmap)
{
    vertex_descriptor u = boost::graph_traits<Graph>::null_vertex();
    vertex_descriptor v;
    for (std::size_t i = 0; i < poly.size(); i++) {
        // check if the point is not yet in the graph
        if (pvmap.find(poly[i]) == pvmap.end()) {
            v = add_vertex(graph);
            pvmap[poly[i]] = v;
        }
        else {
            v = pvmap[poly[i]];
        }
        graph[v] = poly[i];  // associate the point to the vertex
        if (i != 0) {
            add_edge(u, v, graph);
        }
        u = v;
    }
}

template <typename Graph>
struct Polyline_visitor
{
  std::list<Polyline_2>& polylines;
  const Graph& points_pmap;

  Polyline_visitor(std::list<Polyline_2>& lines,
                   const Graph& points_property_map)
    : polylines(lines),
      points_pmap(points_property_map)
  {}

  void start_new_polyline()
  {
    Polyline_2 V;
    polylines.push_back(V);
  }

  void add_node(typename boost::graph_traits<Graph>::vertex_descriptor vd)
  {
    Polyline_2& polyline = polylines.back();
    polyline.push_back(points_pmap[vd]);
  }

  void end_polyline()
  {}

};

int main()
{
  Polyline_2 polyA = { Point_2(0,0), Point_2(1,0), Point_2(2,0), Point_2(3,0), Point_2(4,0)};
  Polyline_2 polyB = { Point_2(1,-1), Point_2(1,0), Point_2(2,0), Point_2(2,1), Point_2(2,2) };

  Graph graph;
  Point_vertex_map pvmap;

  insert(polyA, graph, pvmap);
  insert(polyB, graph, pvmap);

  std::list<Polyline_2> polylines;
  Polyline_visitor<Graph> polyline_visitor(polylines, graph);

  CGAL::split_graph_into_polylines( graph,
                                    polyline_visitor);

  for(std::list<Polyline_2>::iterator it = polylines.begin(); it!= polylines.end(); ++it){
     Polyline_2& poly = *it;
     std::size_t n;
     if(poly.front() == poly.back()){
       std::cout << "POLYGON" << std::endl;
       n = poly.size() -1;
     }else{
       std::cout << "POLYLINE" << std::endl;
       n = poly.size();
     }
     for(std::size_t j=0; j < n; j++){
       std::cout << poly[j] << std::endl;
     }
     std::cout << std::endl;
   }

  return 0;

}


#include <CGAL/Simple_cartesian.h>
#include <CGAL/Surface_mesh.h>

#include <boost/graph/kruskal_min_spanning_tree.hpp>

#include <iostream>
#include <fstream>
#include <list>

typedef CGAL::Simple_cartesian<double>                       Kernel;
typedef Kernel::Point_3                                      Point;
typedef CGAL::Surface_mesh<Point>                            Mesh;

typedef boost::graph_traits<Mesh>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<Mesh>::vertex_iterator   vertex_iterator;
typedef boost::graph_traits<Mesh>::edge_descriptor   edge_descriptor;

void kruskal(const Mesh& sm)
{
   // We use the default edge weight which is the squared length of the edge

  std::list<edge_descriptor> mst;

  boost::kruskal_minimum_spanning_tree(sm,
                                       std::back_inserter(mst));

  std::cout << "#VRML V2.0 utf8\n"
    "Shape {\n"
    "  appearance Appearance {\n"
    "    material Material { emissiveColor 1 0 0}}\n"
    "    geometry\n"
    "    IndexedLineSet {\n"
    "      coord Coordinate {\n"
    "        point [ \n";

  vertex_iterator vb,ve;
  for(boost::tie(vb, ve) = vertices(sm); vb!=ve; ++vb){
    std::cout <<  "        " << sm.point(*vb) << "\n";
  }

  std::cout << "        ]\n"
               "     }\n"
    "      coordIndex [\n";

  for(std::list<edge_descriptor>::iterator it = mst.begin(); it != mst.end(); ++it)
  {
    edge_descriptor e = *it ;
    vertex_descriptor s = source(e,sm);
    vertex_descriptor t = target(e,sm);
    std::cout << "      " << s << ", " << t <<  ", -1\n";
  }

  std::cout << "]\n"
    "  }#IndexedLineSet\n"
    "}# Shape\n";
}

int main(int argc, char** argv)
{
  Mesh sm;
  if(argc < 2 || !CGAL::IO::read_polygon_mesh(argv[1], sm))
  {
    std::cerr << "Invalid input file." << std::endl;
    return EXIT_FAILURE;
  }

  kruskal(sm);

  return 0;
}


/*
    The Fitter is responsible for managing bookkeeping of the curves, their objectives
    and correctly composing the sparse jacobian and residual vector used by the
    non-linear least squares solver.
*/
#pragma once

// polyvec
#include <polyvec/curve-tracer/curve_objective.hpp>
#include <polyvec/curve-tracer/curve_parametrization.hpp>
#include <polyvec/curve-tracer/curve_constraint.hpp>

// libc++
#include <vector>
#include <memory>

// Eigen
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <unsupported/Eigen/LevenbergMarquardt> // SparseFunctor

NAMESPACE_BEGIN(polyvec)

// Forward declarations
class GlobFitObjective;
class GlobFitCurve;

struct GlobFitter : public Eigen::SparseFunctor<double, int> {
public:
	GlobFitter() : Eigen::SparseFunctor<double, int>(0, 0) { }
    void set_curves ( const std::vector<GlobFitCurveParametrization*>& );
    void set_objectives ( const std::vector<GlobFitObjective*>& );

	//Adds hard constraints to the optimization problem. The following requirements must be
	//met for constraints  target = f(source):
	//  * target cannot be coupled to other parameters
	//  * target cannot be fixed
	//  * none of source can be a target of a constraint
	void set_constraints(const std::vector<GlobFitConstraint*>&);

	//Prepares the optimization process
    void setup ( const int max_iterations );

	//returns the number of iterations
    int run_fitter ( FILE* log_file, std::function<void ( int ) > callback );

	void report_errors( FILE* error_file );

    int n_parameters();
    int n_equations();
    bool is_setup();

    std::vector<GlobFitCurveParametrization*> get_curves();
    std::vector<GlobFitObjective*> get_objectives();

private:

	void compute_objective_and_jacobian(Eigen::VectorXd& obj, Eigen::SparseMatrix<double>& dobj_dcurveparams); // + matrix

	//extracts the current set of variables from the current state of the curves
	Eigen::VectorXd get_variables();

	//updates the curves with the given set of variables
	void set_variables(const Eigen::VectorXd&);

	int nextParameter = 0;
    std::vector<GlobFitCurveParametrization*> _curves;
	std::map< GlobFitCurveParametrization*, size_t> curve_to_index;
	std::vector<GlobFitObjective*> _objectives;
	std::vector<GlobFitConstraint*> _constraints;
	std::vector<Eigen::VectorXd> constraint_gradients;

    bool _is_setup = false;

	//id of the first equation of the i-th objective
    std::vector<int> _xadj_equations = std::vector<int>();
	std::vector<std::vector<int>> variables_in_objective;
    int _max_iterations;

    Eigen::VectorXd _cached_params;
    Eigen::VectorXd _cached_objective;
    Eigen::SparseMatrix<double> _cached_jacobian;

	void check_derivatives();

    // ============== API needed by Eigen Levenberg-Marquart
public:

    int inputs();
    int values();
    int operator() ( const Eigen::VectorXd& x, Eigen::VectorXd& fvec );
    int df ( const Eigen::VectorXd& x, Eigen::SparseMatrix<double>& fjac );
};

NAMESPACE_END (polyvec)


#include "Chapter3_SparseLinearAlgebra/Section1_SparseMatrixManipulations.hpp"
#include <Eigen/Geometry>
void TestChapter3Section1()
{
        Chapter3_SparseLinearAlgebra::Section1_SparseMatrixManipulations::FirstExample();
}

void TestChapter3Section2()
{

        Chapter3_SparseLinearAlgebra::Section1_SparseMatrixManipulations::TheSparseMatrixClass();
}

int main()
{
        //build pass
        TestChapter3Section1();   // 会生成一张图片，titiled  "result.bmp"

        TestChapter3Section2();
        return 0;
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include "Utils/Geometry/StructuralCompletion.h"
#include <Eigen/Geometry>

namespace Scine {
namespace Utils {

void StructuralCompletion::generate3TetrahedronCornersFrom1Other(const Eigen::Vector3d& v1, Eigen::Ref<Eigen::Vector3d> v2,
                                                                 Eigen::Ref<Eigen::Vector3d> v3,
                                                                 Eigen::Ref<Eigen::Vector3d> v4) {
  /*
   * Generate a vector perpendicular to the given one and then perform rotation around this vector to generate v2, then
   * rotate v2 around v1 to generate v3 and v4.
   */

  // Get a perpendicular vector;
  Eigen::Vector3d perpendicularVector = v1.cross(Eigen::Vector3d(1, 0, 0));
  // Check that it was different enough from {1,0,0}; if not, create it from {0,1,0}
  if (perpendicularVector.squaredNorm() < 0.00001) {
    perpendicularVector = v1.cross(Eigen::Vector3d(0, 1, 0));
  }
  perpendicularVector.normalize();

  Eigen::AngleAxisd t(tetrahedronAngle, perpendicularVector);
  auto R = t.toRotationMatrix();
  v2 = R * v1;
  double ang2 = 120.0 * Constants::rad_per_degree;
  Eigen::AngleAxisd t2(ang2, v1);
  R = t2.toRotationMatrix();
  v3 = R * v2;
  v4 = R * v3;
}

void StructuralCompletion::generate2TetrahedronCornersFrom2Others(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2,
                                                                  Eigen::Ref<Eigen::Vector3d> v3,
                                                                  Eigen::Ref<Eigen::Vector3d> v4) {
  /*
   * Generate the vector a, which is in the middle between v1 and v2, and the vector b, which is perpendicular to a but
   * in the same plane as v1 and v2, and then rotate a around b to generate v3 and v4.
   */

  Eigen::Vector3d a = (v1 + v2).normalized();
  Eigen::Vector3d b = (v2 - v1).normalized();

  double angle = (360 * Constants::rad_per_degree - tetrahedronAngle) / 2;
  Eigen::AngleAxisd t(angle, b);
  auto R = t.toRotationMatrix();
  v3 = R * a;
  v4 = R * v3;
}

void StructuralCompletion::generate1TetrahedronCornerFrom3Others(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2,
                                                                 const Eigen::Vector3d& v3, Eigen::Ref<Eigen::Vector3d> v4) {
  Eigen::Vector3d res = -(v1 + v2 + v3);
  // check that the three vectors were non-planar enough
  if (res.squaredNorm() < 0.4 * 0.4) {
    res = v1.cross(v2);
  }

  v4 = res.normalized();
}

void StructuralCompletion::generate2TriangleCornersFrom1Other(const Eigen::Vector3d& v1, Eigen::Ref<Eigen::Vector3d> v2,
                                                              Eigen::Ref<Eigen::Vector3d> v3) {
  /*
   * Generate a vector perpendicular to the given one and then perform rotation around this vector to generate v2 and
   * v3.
   */

  // Get a perpendicular vector;
  auto perpendicularVector = v1.cross(Eigen::Vector3d(1, 0, 0));
  // Check that it was different enough from {1,0,0}; if not, create it from {0,1,0}
  if (perpendicularVector.squaredNorm() < 0.00001) {
    perpendicularVector = v1.cross(Eigen::Vector3d(0, 1, 0));
  }
  perpendicularVector.normalize();

  Eigen::AngleAxisd t(120 * Constants::rad_per_degree, perpendicularVector);
  auto R = t.toRotationMatrix();
  v2 = R * v1;
  v3 = R * v2;
}

void StructuralCompletion::generate1TriangleCornerFrom2Others(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2,
                                                              Eigen::Ref<Eigen::Vector3d> v3) {
  v3 = -(v1 + v2).normalized();
}

constexpr double StructuralCompletion::tetrahedronAngle;

} /* namespace Utils */
} /* namespace Scine */


#include <iostream>
#include <fstream>

#include <opencv2/opencv.hpp>
#include <boost/format.hpp>  // for formating strings
#include <sophus/se3.hpp>

#include "config.hpp"

using namespace std;

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
typedef Eigen::Matrix<double, 6, 1> Vector6d;

void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud);

int main(int argc, char **argv) {
    vector<cv::Mat> colorImgs, depthImgs;  // 彩色图和深度图
    TrajectoryType poses;                  // 相机位姿

    ifstream fin((string(CURREXAMPLE_ROOT) + string("/pose.txt")).c_str());
    if (!fin) {
        cerr << "请在有pose.txt的目录下运行此程序" << endl;
        return 1;
    }

    for (int i = 0; i < 5; i++) {
        boost::format fmt((string(CURREXAMPLE_ROOT) + string("/%s/%d.%s")).c_str());  //图像文件格式
        colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
        depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(), -1));  // 使用-1读取原始图像

        double data[7] = {0};
        for (auto &d : data)
            fin >> d;
        Sophus::SE3d pose(Eigen::Quaterniond(data[6], data[3], data[4], data[5]),
                          Eigen::Vector3d(data[0], data[1], data[2]));
        poses.push_back(pose);
    }

    // 计算点云并拼接
    // 相机内参
    double cx         = 325.5;
    double cy         = 253.5;
    double fx         = 518.0;
    double fy         = 519.0;
    double depthScale = 1000.0;
    vector<Vector6d, Eigen::aligned_allocator<Vector6d>> pointcloud;
    pointcloud.reserve(1000000);

    for (int i = 0; i < 5; i++) {
        cout << "转换图像中: " << i + 1 << endl;
        cv::Mat color  = colorImgs[i];
        cv::Mat depth  = depthImgs[i];
        Sophus::SE3d T = poses[i];
        for (int v = 0; v < color.rows; v++)
            for (int u = 0; u < color.cols; u++) {
                unsigned int d = depth.ptr<unsigned short>(v)[u];  // 深度值
                if (d == 0) continue;                              // 为0表示没有测量到
                Eigen::Vector3d point;
                point[2]                   = double(d) / depthScale;
                point[0]                   = (u - cx) * point[2] / fx;
                point[1]                   = (v - cy) * point[2] / fy;
                Eigen::Vector3d pointWorld = T * point;

                Vector6d p;
                p.head<3>() = pointWorld;
                p[5]        = color.data[v * color.step + u * color.channels()];      // blue
                p[4]        = color.data[v * color.step + u * color.channels() + 1];  // green
                p[3]        = color.data[v * color.step + u * color.channels() + 2];  // red
                pointcloud.push_back(p);
            }
    }

    cout << "点云共有" << pointcloud.size() << "个点." << endl;
    showPointCloud(pointcloud);
    return 0;
}

/*******************************************************************************************/
#include "common/glfwEntry.hpp"
X3D::X3DGate x3dGate;

void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud) {
    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    GLFWwindow *win = glfwEntry::initialize("stereoVision", 1024, 768, 0.2, 0.3, 0.4, &x3dGate);

    vector<float> vertPos;
    vector<float> vertColor;
    uint32_t ptsObjID = 0;

    vertPos.resize(pointcloud.size() * 3);
    vertColor.resize(vertPos.size());

    uint32_t idx = 0;
    for (auto &p : pointcloud) {
        vertPos[idx * 3]     = p[0];
        vertPos[idx * 3 + 1] = p[1];
        vertPos[idx * 3 + 2] = p[2];

        vertColor[idx * 3]     = p[3] / 255.0;
        vertColor[idx * 3 + 1] = p[4] / 255.0;
        vertColor[idx * 3 + 2] = p[5] / 255.0;

        ++idx;
    }

    x3dGate.AddPoints("userPoints", vertPos, vertColor, 5,
                      {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f},
                      [&](uint32_t objID) {
                          if (ptsObjID == 0) {
                              ptsObjID = objID;
                          }
                      });

    bool show_debug = false;
    while (!glfwWindowShouldClose(win)) {
        glfwEntry::beginFrame(win);

        {
            ImGui::Begin("Test info");

            ImGui::Text("Application average %.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, ImGui::GetIO().Framerate);

            if (ImGui::Button("Toggle debug")) {
                x3dGate.ToggleDebug();
                show_debug = !show_debug;
            }
            ImGui::End();
        }

        glfwEntry::endFrame(win);
    }

    x3dGate.DeleteObject(ptsObjID, [](bool ret) {});

    glfwEntry::finalize();

    return;
}


#pragma once

#include <vector>
#include <set>
#include <list>
#include <string>
#include <limits>
#include <fstream>
#include <Eigen/Eigenvalues>
#include <open3d/geometry/BoundingVolume.h>
#include <open3d/geometry/Geometry3D.h>
#include <open3d/geometry/PointCloud.h>
#include <open3d/geometry/Octree.h>
#include <open3d/geometry/KDTreeFlann.h>
#include <open3d/utility/Console.h>
#include <open3d/io/PointCloudIO.h>
#include "json.hpp"
#include "glasbey_lut.hpp"

namespace open3d {

namespace geometry {

class SuperVoxel;

using nlohmann::json;

#define ASSERT(p) if(!(p)) {std::cout << __FILE__ ":" << __LINE__ << " " #p << std::endl; throw #p;}

static inline std::set<int> envvar(const char* name)
{
    const char* v = std::getenv(name);
    if (v) {
		std::string s(v);
		std::set<int> ret;
		std::stringstream ss(s);
		std::string item;
		while (getline(ss, item, ',')) {
			if (!item.empty()) {
				ret.insert(stoi(item));
			}
		}
		return ret;
	}
    return std::set<int>();
}

static inline int envvar(const char* name, int def)
{
    const char* v = std::getenv(name);
    if (v) {
        return std::stoi(v);
    }
    return def;
}

static inline double envvar(const char* name, double def)
{
    const char* v = std::getenv(name);
    if (v) {
        return std::stof(v);
    }
    return def;
}

static inline json vec2json(const Eigen::Vector3d v)
{
    return json::array({v(0), v(1), v(2)});
}

static inline Eigen::Vector3d rgb2xyz(const Eigen::Vector3d& rgb) {
    Eigen::Vector3d srgb;
    srgb(0)  = rgb(0) > 0.04045 ? std::pow((rgb(0) + 0.055) / 1.055, 2.4) : rgb(0) / 12.92;
    srgb(1)  = rgb(1) > 0.04045 ? std::pow((rgb(1) + 0.055) / 1.055, 2.4) : rgb(1) / 12.92;
    srgb(2)  = rgb(2) > 0.04045 ? std::pow((rgb(2) + 0.055) / 1.055, 2.4) : rgb(2) / 12.92;
    return Eigen::Vector3d(
        (srgb[0] * 0.4124) + (srgb[1] * 0.3576) + (srgb[2] * 0.1805),
        (srgb[0] * 0.2126) + (srgb[1] * 0.7152) + (srgb[2] * 0.0722),
        (srgb[0] * 0.0193) + (srgb[1] * 0.1192) + (srgb[2] * 0.9505)
    );
}

static inline Eigen::Vector3d xyz2lab(const Eigen::Vector3d& xyz) {
    Eigen::Vector3d xyzD;
    xyzD(0) = xyz(0) > 0.008856 ? std::cbrt(xyz(0)) : 7.787 * xyz(0) + 16 / 116;
    xyzD(1) = xyz(1) > 0.008856 ? std::cbrt(xyz(1)) : 7.787 * xyz(1) + 16 / 116;
    xyzD(2) = xyz(2) > 0.008856 ? std::cbrt(xyz(2)) : 7.787 * xyz(2) + 16 / 116;
    return Eigen::Vector3d((116.0 * xyzD[1]) - 16.0, 500.0 * (xyzD[0] - xyzD[1]), 200.0 * (xyzD[1] - xyzD[2]));
}

static inline Eigen::Vector3d rgb2lab(const Eigen::Vector3d& rgb) {
    return xyz2lab(rgb2xyz(rgb));
}

// average filter of region property
struct RegionProperty {
public:
    RegionProperty()
        : color_(Eigen::Vector3d::Zero())
        , point_(Eigen::Vector3d::Zero())
        , normal_(Eigen::Vector3d::Zero())
        , num_of_points_(0)
        {}

    RegionProperty(const Eigen::Vector3d& color, const Eigen::Vector3d& point, const Eigen::Vector3d& normal)
        : color_(color)
        , point_(point)
        , normal_(normal)
        , num_of_points_(1)
        {}

    inline RegionProperty operator+(const RegionProperty& rhs) const
    {
        RegionProperty ret;
        ret.color_  = color_  + rhs.color_ ;
        ret.point_  = point_  + rhs.point_ ;
        ret.normal_ = normal_ + rhs.normal_;
        ret.num_of_points_ = num_of_points_ + rhs.num_of_points_;
        return ret;
    }

    inline RegionProperty operator-(const RegionProperty& rhs) const
    {
        RegionProperty ret;
        ret.color_  = color_  - rhs.color_ ;
        ret.point_  = point_  - rhs.point_ ;
        ret.normal_ = normal_ - rhs.normal_;
        ret.num_of_points_ = num_of_points_ - rhs.num_of_points_;
        return ret;
    }

    inline RegionProperty Normalize()
    {
        RegionProperty ret;
        ASSERT(num_of_points_ > 0);
        ret.color_  = color_ / num_of_points_;
        ret.point_  = point_ / num_of_points_;
        ret.normal_ = normal_.normalized();
        ret.num_of_points_ = num_of_points_;
        return ret;
    }

    std::string toString() const
    {
        std::stringstream ss;
        ss << "p [" << point_(0) << ", " << point_(1) << ", " << point_(2) << "]";
        ss << "n [" << normal_(0) << ", " << normal_(1) << ", " << normal_(2) << "]";
        ss << "c [" << color_(0) << ", " << color_(1) << ", " << color_(2) << "]";
        ss << "n :" << num_of_points_;
        return ss.str();
    }

    inline double DistanceToPlane(const Eigen::Vector3d &p) const
    {
        return std::abs(normal_.dot(point_ - p));
    }

public:
    Eigen::Vector3d color_;
    Eigen::Vector3d point_;
    Eigen::Vector3d normal_;
    int num_of_points_;

};

std::function<double(const RegionProperty&, const RegionProperty&)> DistanceFunctionL1(double lambda, double mu, double epsilon)
{
    std::function<double(const RegionProperty&, const RegionProperty&)> f
    = [lambda, mu, epsilon](const RegionProperty& r1, const RegionProperty& r2) -> double{
        double Dc = (r1.color_ - r2.color_).norm();
        double Ds = (r1.point_ - r2.point_).norm();
        double Dn = 1.0f - std::abs(r1.normal_.dot(r2.normal_));
        return  lambda * Dc + mu * Ds + epsilon * Dn;
    };
    return f;
}

std::function<double(const RegionProperty&, const RegionProperty&)> DistanceFunctionLInf(double lambda, double mu, double epsilon)
{
    std::function<double(const RegionProperty&, const RegionProperty&)> f
    = [lambda, mu, epsilon](const RegionProperty& r1, const RegionProperty& r2) -> double{
        double Dc = (r1.color_ - r2.color_).norm();
        double Ds = (r1.point_ - r2.point_).norm();
        double Dn = 1.0f - std::abs(r1.normal_.dot(r2.normal_));
        return  std::max(std::max(lambda * Dc, mu * Ds), epsilon * Dn);
    };
    return f;
}

template <typename T>
void BFS(T root, const std::vector<std::set<T> > &adj, int max_depth, const std::function<bool(const T&)> &f) {

    std::set<T> visited;
    std::list<T> leaves;

    leaves.push_back(root);
    int depth = 0;

    while (leaves.size() > 0) {

        if (depth == max_depth) {
            return;
        }

        std::list<T> new_leaves;

        for (auto leaf : leaves) {

            bool keep_searching = f(leaf);

            if (!keep_searching) {
                continue;
            }

            for (auto neighbor : adj[leaf]) {
                if (visited.find(neighbor) == visited.end()) {
                    new_leaves.push_back(neighbor);
                    visited.insert(neighbor);
                }
            }
        }

        leaves = new_leaves;
        depth++;
    }
}

static Eigen::Vector3d FlipDirection(const Eigen::Vector3d &target, const Eigen::Vector3d reference = Eigen::Vector3d(0, 0, 1), const Eigen::Vector3d second_reference = Eigen::Vector3d(0, 1, 0))
{
    if (target.dot(reference) < 0) {
        return -target;
    }
    if (target.dot(reference) > 0) {
        return target;
    }
    if (target.dot(second_reference) < 0) {
        return -target;
    }
    return target;
}

static Eigen::Vector3d EstimateNormal(
        const geometry::PointCloud& pc,
        const std::vector<int>& indices,
        Eigen::Vector3d &eigenvalues)
{
    if (indices.size() < 3) {
        return Eigen::Vector3d::Zero();
    }

    Eigen::Matrix3d covariance = utility::ComputeCovariance(pc.points_, indices);

    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver;
    solver.computeDirect(covariance, Eigen::ComputeEigenvectors);

    eigenvalues = solver.eigenvalues();

    return FlipDirection(solver.eigenvectors().col(0));
}

using GridKey = int64_t;
using PointID = int;
using VoxelID = int;
using SuperVoxelID = int;

struct SuperVoxel {
public:
    RegionProperty prop_;
    VoxelID seed_;
};

struct Edge {
public:
    Edge(SuperVoxelID svid1, SuperVoxelID svid2, double d) : svid1(svid1), svid2(svid2), d(d) {
        ASSERT(svid1 < svid2);
        ASSERT(d >= 0);
    }
    SuperVoxelID svid1;
    SuperVoxelID svid2;
    double d;
};

struct EdgeCompare {
public:
    inline bool operator()(const Edge& a, const Edge& b) const
    {
        return a.d < b.d;
    }
};

/// \class EVCCS
///
/// \brief Edge-removal Voxel Cloud Connectivity Segmentation.
///
class EVCCS
{
public:
    using LabelType = unsigned int;
    static const SuperVoxelID NO_LABEL = -1;
    static const VoxelID NO_VOXEL = -1;

public:
    /// \brief Default Constructor.
    EVCCS() { };

    ~EVCCS() { }

    // vxl_size : resolution.
    bool Execute(const PointCloud &pc, double vxl_size, int r_seed, int iteration_num, std::function<double(const RegionProperty&, const RegionProperty&)> f, double maxd, double r_adj)
    {
        utility::LogInfo("EVCCS: start");

        if ( !pc.HasColors() ) {
           utility::LogWarning("input point cloud does'nt have colors!");
           return false;
        }

        ASSERT(vxl_size > 0);
        ASSERT(r_seed > 1);

        int r_search = (r_seed+1) / 2;

        Voxelize(pc, vxl_size);

        ScatterSeeds(vxl_size, r_seed, r_search);

        FilterOutSeedsInSparseArea(vxl_size, r_search);

        //ShiftSeeds(r_search);

        Clustering(iteration_num, f, maxd, r_seed, r_adj);

        utility::LogInfo("EVCCS: end");
        return true;
    }

    //debug
public:
    //std::shared_ptr<PointCloud> CreateLabeledVoxelCloud(const std::vector<Eigen::Vector3d>& lut)
    //{
    //    Eigen::Vector3d nolabelColor(0, 0, 0);
    //    Eigen::Vector3d edgeColor(1, 1, 1);
    //    Eigen::Vector3d removalColor(1, 1, 1);
    //    auto pc = std::make_shared<PointCloud>();
    //    for(unsigned int i=0; i<vc_.points_.size(); i++) {
    //        if (static_cast<int>(i) == envvar("DEBUG_IDX", -1)) {
    //            std::cout << vc_.points_[i] << std::endl;
    //            if (vlabel_[i] == NO_LABEL) {
    //                std::cout << "nolabel" << std::endl;
    //            } else {
    //                std::cout << (lut[vlabel_[i] % lut.size()] * 255) << std::endl;
    //            }
    //        }
    //        pc->points_.push_back(vc_.points_[i]);
    //        pc->normals_.push_back(vc_.normals_[i]);
    //        if (vlabel_[i] == NO_LABEL) {
    //            pc->colors_.push_back(nolabelColor);
    //        } else {
    //            pc->colors_.push_back(lut[vlabel_[i] % lut.size()]);
    //        }
    //    }
    //    for(unsigned int i=0; i<evc_.points_.size(); i++) {
    //        pc->points_.push_back(evc_.points_[i]);
    //        pc->normals_.push_back(evc_.normals_[i]);
    //        pc->colors_.push_back(edgeColor);
    //    }
    //    for(unsigned int i=0; i<rvc_.points_.size(); i++) {
    //        pc->points_.push_back(rvc_.points_[i]);
    //        pc->normals_.push_back(rvc_.normals_[i]);
    //        pc->colors_.push_back(removalColor);
    //    }
    //    return pc;
    //}

    //void PaintLabelColor(PointCloud &pc, const std::vector<Eigen::Vector3d>& lut, double r = 0.02)
    //{
    //    Eigen::Vector3d nolabelColor(0, 0, 0);
    //    std::vector<int> vindices;
    //    std::vector<double> vdistance2;

    //    for(unsigned int i=0; i<pc.points_.size(); i++) {
    //        // TODO use normal and color in edge/removed-voxel kdtrree
    //        kdt_.SearchHybrid(pc.points_[i], r, 1, vindices, vdistance2);
    //        if (vindices.size() > 0) {
    //            ASSERT(vlabel_[vindices[0]] != NO_LABEL);
    //            pc.colors_[i] = lut[vlabel_[vindices[0]] % lut.size()];
    //        }
    //    }
    //}

    //void DumpLabel(const PointCloud &pc, std::string filename, double r_adj)
    //{
    //        std::ofstream lo(filename, std::ios_base::binary);
    //        std::vector<int> vindices;
    //        std::vector<double> vdistance2;

    //        for(unsigned int i=0; i<pc.points_.size(); i++) {
    //            int label = NO_LABEL;
    //            kdt_.SearchHybrid(pc.points_[i], r_adj, 1, vindices, vdistance2);
    //            if (vindices.size() > 0) {
    //                label = vlabel_[vindices[0]];
    //            }
    //            lo.write((const char*)&label, sizeof(label));
    //        }
    //}

    SuperVoxelID Lookup(SuperVoxelID label, const std::vector<SuperVoxelID> &ltree, const std::vector<double> &dtree)
    {
        SuperVoxelID prev = label;
        while (true) {
            label = ltree[prev];
            if (label == prev) {
                break;
            }
            prev = label;
        }
        return prev;
    }

    void DumpLabeledPLY(const PointCloud &pc, std::string filename, const std::vector<SuperVoxelID> &ltree, const std::vector<double> &dtree, int min_point_num)
    {
        utility::LogInfo("EVCCS: DumpLabeledPLY start");
        {
            std::ofstream o(filename, std::ios_base::binary);
            if (o.is_open()) {
                o << "ply" << std::endl;
                o << "format binary_little_endian 1.0" << std::endl;
                o << "element vertex " << pc.points_.size() << std::endl;
                o << "property float x" << std::endl;
                o << "property float y" << std::endl;
                o << "property float z" << std::endl;
                o << "property uchar red" << std::endl;
                o << "property uchar green" << std::endl;
                o << "property uchar blue" << std::endl;
                o << "property int32 label" << std::endl;
                o << "property uchar lred" << std::endl;
                o << "property uchar lgreen" << std::endl;
                o << "property uchar lblue" << std::endl;
                o << "end_header" << std::endl;
            } else {
                std::cout << "cannot open " << filename << std::endl;
                return;
            }
        }

		std::vector<SuperVoxelID> lut(svxls_.size());
		for (unsigned int i=0; i<lut.size(); i++) {
			lut[i] = Lookup(i, ltree, dtree);
			//std::cout << "lut[" << i << "] : " << lut[i] <<  " : #" <<mergedsvxls_[lut[i]].num_of_points_  << " d:" << dtree[lut[i]] << std::endl;
		}

		PointCloud big_vc;
		std::vector<SuperVoxelID> big_label;
		for (unsigned int i=0; i<vc_.points_.size(); i++ ) {
			auto label = lut[vlabel_[i]];
			//std::cout << "vxl[" << i << "] : " << vlabel_[i] << " -> " << label << " : " << mergedsvxls_[label].num_of_points_  << " big:" << (mergedsvxls_[label].num_of_points_ > min_point_num) << std::endl;
			if (mergedsvxls_[label].num_of_points_ > min_point_num) {
				big_vc.points_.push_back(vc_.points_[i]);
				big_label.push_back(label);
			}
		}
		KDTreeFlann big_kdt(big_vc);

        {
            std::set<int> targets = envvar("TARGET");
			double k = envvar("TARGET_RED", 0.5);
            std::ofstream o(filename, std::ios_base::binary|std::ios_base::app);
            std::vector<int> vindices;
            std::vector<double> vdistance2;

            for(unsigned int i=0; i<pc.points_.size(); i++) {
                int label = NO_LABEL;
                big_kdt.SearchKNN(pc.points_[i], 1, vindices, vdistance2);
				ASSERT(vindices.size() > 0);
				label = big_label[vindices[0]];
                for (int j=0; j<3; j++) {
                    float x = pc.points_[i](j);
                    o.write((const char*)&x, sizeof(x));
                }
                auto color = pc.colors_[i];
                if (targets.find(label) != targets.end()) {
                    //color(0) *= 0.5;
                    color(1) *= k;
                    color(2) *= k;
                }
                for (int j=0; j<3; j++) {
                    unsigned char x = 255*color(j);
                    o.write((const char*)&x, sizeof(x));
                }
                o.write((const char*)&label, sizeof(label));
                auto labelcolor = GLASBEY_LUT[label % GLASBEY_LUT.size()];
                for (int j=0; j<3; j++) {
                    unsigned char x = 255 * labelcolor(j);
                    o.write((const char*)&x, sizeof(x));
                }
            }
        }
        utility::LogInfo("EVCCS: DumpLabeledPLY end");
    }

    void DumpSupervoxels(std::string filename)
    {
        json j;
        for (unsigned int svid=0; svid<svxls_.size(); svid++) {
            if (svxls_[svid]->seed_ == NO_VOXEL) {
                continue;
            }
            ASSERT(svxls_[svid]->prop_.num_of_points_ > 0);
            auto prop = svxls_[svid]->prop_.Normalize();
            j["node"][svid]["numOfPoints"] = prop.num_of_points_;
            j["node"][svid]["color"] = prop.color_;
            j["node"][svid]["point"] = prop.point_;
            j["node"][svid]["normal"] = prop.normal_;
        }

        std::ofstream o(filename);
        if (o.is_open()) {
            o << j;
        } else {
            std::cout << "cannot open " << filename << std::endl;
        }

    }

    void CreateLabelTree(
        std::vector<SuperVoxelID> &labeltree,
        std::vector<double> &disttree,
        std::function<double(const RegionProperty&, const RegionProperty&)> f,
		double  maxd
    )
    {
        utility::LogInfo("EVCCS: LabelTree start");

		mergedsvxls_.clear();
        labeltree.clear();
        disttree.clear();

        for (SuperVoxelID svid=0; svid<svxls_.size(); svid++) {
            if (svxls_[svid]->seed_ == NO_VOXEL) {
                mergedsvxls_.push_back(RegionProperty());
                labeltree.push_back(-1);
            } else {
                mergedsvxls_.push_back(svxls_[svid]->prop_);
                labeltree.push_back(svid);
            }
            disttree.push_back(0);
        }

        auto svadj = svadj_;
        std::multiset<Edge, EdgeCompare> edges;
        edges.clear();
        for (auto es : svadj) {
            SuperVoxelID svid1 = es.first;
            for (auto svid2 : es.second) {
                if (svid1 >= svid2) {
                    continue;
                }
                double d = f(mergedsvxls_[svid1], mergedsvxls_[svid2]);
                edges.insert(Edge(svid1, svid2, d));
                //std::cout << "init edge " << svid1 << " - " << svid2 << std::endl;
            }
        }

        while (edges.size() > 0) {

            auto e = edges.begin();
            edges.erase(e);
            auto svid1 = e->svid1;
            auto svid2 = e->svid2;
            ASSERT(svid1 <= labeltree[svid1]);
            ASSERT(svid2 <= labeltree[svid2]);
            //already merged
            if (svid1 != labeltree[svid1] || svid2 != labeltree[svid2]) {
                //std::cout << "ignore " << svid1 << "(->" <<  labeltree[svid1] << ") - " << svid2 << "(->" << labeltree[svid2] << ")" << std::endl;
                //std::cout << "ignoreDebug " << svid1 << " : " << labeltree[svid1] << " @ " << &labeltree[svid1] << std::endl;
                //std::cout << "ignoreDebug " << svid2 << " : " << labeltree[svid2] << " @ " << &labeltree[svid2] << std::endl;
                continue;
            }
			if (e->d > maxd) {
				break;
			}
            SuperVoxelID newID = mergedsvxls_.size();
            mergedsvxls_.push_back(mergedsvxls_[svid1] + mergedsvxls_[svid2]);

            if (envvar("DUMP_LABELTREE", 0)) {
                std::cout << "relabel " << svid1 << " (#" << mergedsvxls_[svid1].num_of_points_ << ") + " << svid2 << " (#" << mergedsvxls_[svid2].num_of_points_ << ") -> " << newID << " (#" << mergedsvxls_[newID].num_of_points_ << ") : " << e->d << std::endl;
            }

            labeltree[svid1] = newID;
            labeltree[svid2] = newID;
            labeltree.push_back(newID);
            //std::cout << "mergeDebug " << svid1 << " : " << labeltree[svid1] << " @ " << &labeltree[svid1] << std::endl;
            //std::cout << "mergeDebug " << svid2 << " : " << labeltree[svid2] << " @ " << &labeltree[svid2] << std::endl;
            //std::cout << "mergeDebug " << newID << " : " << labeltree[newID] << " @ " << &labeltree[newID] << std::endl;
            disttree.push_back(e->d);

            svadj[newID] = std::set<SuperVoxelID>();

            for (auto n1 : svadj[svid1] ) {
                if (n1 == svid2) { continue; }
                svadj[newID].insert(n1);
                svadj[n1].erase(svid1);
                svadj[n1].insert(newID);
                //std::cout << "adj : @" << svid1 << " " << n1 << "->" << newID << std::endl;
            }
            svadj.erase(svid1);

            for (auto n2 : svadj[svid2] ) {
                if (n2 == svid1) { continue; }
                svadj[newID].insert(n2);
                svadj[n2].erase(svid2);
                svadj[n2].insert(newID);
                //std::cout << "adj : @" << svid2 << " " << n2 << "->" << newID << std::endl;
            }
            svadj.erase(svid2);

            for (auto n : svadj[newID]) {
                double d = f(mergedsvxls_[newID].Normalize(), mergedsvxls_[n].Normalize());
                //std::cout << "edge : " << n << " " << newID << " " << d << std::endl;
                edges.insert(Edge(n, newID, d));
            }
            newID++;
        }
        utility::LogInfo("EVCCS: LabelTree end");
    }

protected:

    void Voxelize(const PointCloud &pc, double vxl_size, double size_expand = 0.01) {

        utility::LogInfo("voxelize");

        Eigen::Array3d min_bound = pc.GetMinBound();
        Eigen::Array3d max_bound = pc.GetMaxBound();
        Eigen::Array3d center = (min_bound + max_bound) / 2;
        Eigen::Array3d half_sizes = center - min_bound;
        double max_half_size = half_sizes.maxCoeff();
        Eigen::Vector3d origin = min_bound.min(center - max_half_size); // grid is isotropic box

        double size;
        if (max_half_size == 0) {
            size = size_expand;
        } else {
            size = max_half_size * 2 * (1 + size_expand);
        }

        double voxel_num = std::log2(std::ceil(size / vxl_size) - 1);
        const int MAX_VOXEL_NUM = std::numeric_limits<short>::max();
        if (voxel_num > MAX_VOXEL_NUM) {
            utility::LogWarning("vxl_size is too small.");
            while (voxel_num > MAX_VOXEL_NUM) {
                vxl_size *= 2;
                voxel_num = std::log2(std::ceil(size / vxl_size) - 1);
            }
            utility::LogWarning("vxl_size is clipped.");
        }

        int grid_size = 1 << (static_cast<int>(voxel_num) + 1);

        auto index = [origin, vxl_size](const Eigen::Vector3d& point) -> Eigen::Vector3i { return ((point - origin) / vxl_size).cast<int>(); };
        auto toKey = [](const Eigen::Vector3i& v) -> GridKey { return (static_cast<GridKey>(v(0)) << 32) + (static_cast<GridKey>(v(1)) << 16) + v(2); };

        std::map<GridKey, std::vector<PointID> > voxels;

        for (unsigned int i = 0; i < pc.points_.size(); i++) {
            auto point = pc.points_[i];
            GridKey key = toKey(index(point));
            voxels[key].push_back(i);
        }

        std::map<GridKey, VoxelID> idtbl;
        std::map<GridKey, VoxelID> idtbl_removed;

        utility::LogInfo("initialize voxel point cloud and index table");
        {
            vc_.Clear();
            //evc_.Clear();
            rvc_.Clear();

            for(auto p:voxels){
                GridKey key = p.first;
                auto indeces = p.second;
                ASSERT(indeces.size() > 0);
                Eigen::Vector3d eigenvalues(0, 0, 0);
                auto normal = EstimateNormal(pc, indeces, eigenvalues);

                Eigen::Vector3d color(0, 0, 0);
                Eigen::Vector3d point(0, 0, 0);
                for (auto i : indeces) {
                    color = color + pc.colors_[i];
                    point = point + pc.points_[i];
                }
                color = color / indeces.size();
                point = point / indeces.size();

                double var = 0;
                for (auto i : indeces) {
                    auto d = pc.colors_[i]-color;
                    var = var + d.dot(d);
                }
                var /= indeces.size();

                if (normal == Eigen::Vector3d::Zero() || eigenvalues(1) <= 0 ||  eigenvalues(2) <= 0) {
                    idtbl_removed[key] = rvc_.points_.size();
                    rvc_.points_.push_back(point);
                    rvc_.colors_.push_back(color);
                    continue;
                } else if (var >= envvar("THRES_COLOR_VAR", 0.05)) {
                    idtbl_removed[key] = rvc_.points_.size();
                    rvc_.points_.push_back(point);
                    rvc_.colors_.push_back(color);
                    continue;
                } else if (std::abs(eigenvalues(0) / eigenvalues(1)) >= envvar("THRES_LAMBDA", 0.01) || std::abs(eigenvalues(1) / eigenvalues(2)) < envvar("THRES_LAMBDA", 0.01)) {
                    idtbl_removed[key] = rvc_.points_.size();
                    rvc_.points_.push_back(point);
                    rvc_.colors_.push_back(color);
                    continue;
                } else if (eigenvalues(0) / (eigenvalues(0) + eigenvalues(1) + eigenvalues(2)) >= envvar("MAX_CURVATURE", 0.002)) {
                    idtbl_removed[key] = rvc_.points_.size();
                    rvc_.points_.push_back(point);
                    rvc_.colors_.push_back(color);
                    continue;
                } else {
                    idtbl[key] = vc_.points_.size();
                    vc_.points_.push_back(point);
                    vc_.colors_.push_back(color);
                    vc_.normals_.push_back(normal);
                    continue;
                }
            }
        }

        for (auto ir:idtbl_removed) {
            ASSERT(idtbl.find(ir.first) == idtbl.end());
            idtbl[ir.first]= ir.second + vc_.points_.size();
        }
        //std::cout << idtbl.size() << " " << vc_.points_.size() << " " << rvc_.points_.size() << std::endl;
        ASSERT(idtbl.size() == vc_.points_.size() + rvc_.points_.size());

        ASSERT(vc_.points_.size() <  std::numeric_limits<VoxelID>::max());
        ASSERT(vc_.points_.size() > 0);

        utility::LogInfo("construct kdtree");
        kdt_.SetGeometry(vc_);

        utility::LogInfo("constract voxel adjacency graph");
        rvadj_.clear();
        rvadj_.resize(idtbl.size());
        vadj_.clear();
        vadj_.resize(vc_.points_.size());
        for(unsigned int i=0; i< idtbl.size(); i++){
            Eigen::Vector3i idx;
            if (i < vc_.points_.size()) {
                idx = index(vc_.points_[i]);
            } else {
                idx = index(rvc_.points_[i - vc_.points_.size()]);
            }
            GridKey key = toKey(idx);
            ASSERT(idtbl[key] == static_cast<int>(i));
            for (int x=-1; x<=1; x++) {
                if (idx(0) + x < 0 || idx(0) + x >= grid_size) { continue; }
                for (int y=-1; y<=1; y++) {
                    if (idx(1) + y < 0 || idx(1) + y >= grid_size) { continue; }
                    for (int z=-1; z<=1; z++) {
                        if (idx(2) + z < 0 || idx(2) + z >= grid_size) { continue; }

                        Eigen::Vector3i neighboridx = idx + Eigen::Vector3i(x, y, z);
                        if (idx == neighboridx) { continue; }

                        GridKey nkey = toKey(neighboridx);

                        if (idtbl.find(nkey) != idtbl.end()) {
                            VoxelID nvxlid = idtbl[nkey];
                            if (nvxlid != static_cast<int>(i)) {
                                if (i < vc_.points_.size() && nvxlid < vc_.points_.size()) {
                                    vadj_[nvxlid].insert(i);
                                    vadj_[i].insert(nvxlid);
                                }
                                rvadj_[nvxlid].insert(i);
                                rvadj_[i].insert(nvxlid);
                            }
                        }
                    }
                }
            }
        }

        if (envvar("DUMP_ADJ", 0)) {
            for (unsigned int i=0; i<vadj_.size(); i++) {
                for (auto n : vadj_[i]) {
                    std::cout << "adj " << i << " -> " << n << std::endl;
                }
            }
        }

    }

    virtual void ScatterSeeds(double vxl_size, int r_seed, int r_search)
    {
        utility::LogInfo("scatter super voxel seeds");

        //auto center = vc_.GetCenter();
        auto center = (vc_.GetMaxBound() + vc_.GetMinBound()) / 2;
        auto extent = vc_.GetMaxBound() - vc_.GetMinBound();
        double r = vxl_size*r_seed;
        int xn2 = extent(0)/(2*r) + 1;
        int yn2 = extent(1)/(2*r) + 1;
        int zn2 = extent(2)/(2*r) + 1;

        std::set<VoxelID> seeds;
        for (int x=-xn2; x<=xn2; x++) {
            for (int y=-yn2; y<=yn2; y++) {
                for (int z=-zn2; z<=zn2; z++) {

                    Eigen::Vector3d point = center + Eigen::Vector3d(x*r, y*r, z*r);

                    std::vector<int> indices;
                    std::vector<double> distance2;

                    int k = kdt_.SearchHybrid(point, r, 1, indices, distance2);
                    if (k == 0) {
                        continue;
                    }
                    ASSERT(indices.size() == 1);
                    seeds.insert(indices[0]);
                }
            }
        }

        for (auto seed : seeds) {
            auto svxl = std::make_shared<SuperVoxel>();
            svxl->seed_ = seed;
            svxl->prop_ = MakeRegionProperty(seed);
            svxls_.push_back(svxl);
        }
    }

    RegionProperty MakeRegionProperty(VoxelID vid)
    {
        return RegionProperty(vc_.colors_[vid], vc_.points_[vid], vc_.normals_[vid]);
    }

    // NOTE: coeff == 0.1 in PCL implementation.
    virtual void FilterOutSeedsInSparseArea(double vxl_size, int r_search, float coeff = 0.2f)
    {
        utility::LogInfo("filter out super voxel seeds");
        float min_points = coeff * r_search * r_search * 3.1415926536f / 2;
        std::vector<std::shared_ptr<SuperVoxel> > new_svxls;

        for(auto svxl : svxls_) {
            std::vector<int> indices;
            std::vector<double> distance2;

            int k = kdt_.SearchRadius(svxl->prop_.point_, vxl_size * r_search, indices, distance2);

            if (k > min_points) {
                new_svxls.push_back(svxl);
                continue;
            }
        }
        svxls_ = new_svxls;
    }

    virtual void UpdateSeed()
    {
        for(auto svxl : svxls_) {
            if (svxl->seed_ == NO_VOXEL) {
                continue;
            }
            std::vector<int> indices;
            std::vector<double> distance2;
            kdt_.SearchKNN(svxl->prop_.Normalize().point_, 1, indices, distance2);
            if (vc_.points_[svxl->seed_] == svxl->prop_.point_) {
                ASSERT(svxl->seed_ == indices[0]);
            }
            svxl->seed_ = indices[0];
            svxl->prop_ = MakeRegionProperty(indices[0]);
        }
    }

    void Clustering(int iteration_num, std::function<double(const RegionProperty&, const RegionProperty&)> f, double max_d, int r_seed, double r_adj)
    {
        utility::LogInfo("clustering voxels into supervoxels");

        for (int iteration=0; iteration<iteration_num; iteration++) {
            UpdateSeed();

            std::vector<SuperVoxelID> v2l(vc_.points_.size(), NO_LABEL);; // voxelid -> supervoxel
            std::vector<double> v2d(vc_.points_.size(), std::numeric_limits<double>::infinity()); // voxelid -> minimum distance

            std::vector<std::vector<VoxelID> > sv2q(svxls_.size()); // search queues for each super voxel
            std::vector<RegionProperty> sv2p(svxls_.size()); // supervoxel -> new region property
            std::vector<std::set<VoxelID> > visited(svxls_.size());

            // initialize svxl propery, searchqueue, label table
            for (unsigned int i=0; i<svxls_.size(); i++) {
                SuperVoxelID svxlid = i;
                auto svxl = svxls_[svxlid];
                if (svxl->seed_ == NO_VOXEL) {
                    continue;
                }
                visited[svxlid].insert(svxl->seed_);
                sv2q[svxlid].push_back(svxl->seed_);
            }

            // kmean
            int depth = 0;
            while (true) {
                bool leaf_appended = false;
                // 1-layer bfs
                for (unsigned int i=0; i<svxls_.size(); i++) {
                    SuperVoxelID svxlid = i;

                    if (svxls_[svxlid]->seed_ == NO_VOXEL) {
                        continue;
                    }
                    if (svxls_[svxlid]->prop_.num_of_points_ == 0) {
                        svxls_[svxlid]->seed_ = NO_VOXEL;
                        continue;
                    }

                    auto cur_svxl_prop = svxls_[svxlid]->prop_.Normalize();

                    std::vector<VoxelID> newq;
                    auto leaves = sv2q[svxlid];

                    for (auto vid : leaves) {
                        auto prop = MakeRegionProperty(vid);
                        auto d = f(prop, cur_svxl_prop);
                        if (d > max_d) {
                            continue;
                        }
                        if (v2d[vid] > d) {
                            v2d[vid] = d;
                            if (v2l[vid] != NO_LABEL) {
                                ASSERT(svxlid != v2l[vid]);
                                auto old_svxlid = v2l[vid];
                                sv2p[old_svxlid] = sv2p[old_svxlid] - prop;
                            }
                            for (auto n : vadj_[vid]) {
                                if (visited[svxlid].find(n) == visited[svxlid].end()) {
                                    newq.push_back(n);
                                    leaf_appended = true;
                                    visited[svxlid].insert(n);
                                }
                            }
                            v2l[vid] = svxlid;
                            sv2p[svxlid] = sv2p[svxlid] + prop;
                        }
                    }

                    sv2q[svxlid] = newq;
                }

                //debug
                if (envvar("DUMP_CLUSTERING", 0)) {
                    std::cout << "---------------------" << std::endl;
                    std::cout << "iteration " << iteration << " depth " << depth << std::endl;
                    for (unsigned int i=0; i<svxls_.size(); i++) {
                        //if (i != 15) { continue; }
                        if (svxls_[i]->seed_ == NO_VOXEL) { continue; }
                        std::cout << "label  " << i  << std::endl;
                        std::cout << "seed " << svxls_[i]->prop_.toString() << std::endl;
                        std::cout << "prop " << sv2p[i].toString() << std::endl;
                        for (auto vid : visited[i]) {
                            if (v2l[vid] == static_cast<int>(i)) {
                                auto x = vc_.points_[vid](0);
                                auto y = vc_.points_[vid](1);
                                auto z = vc_.points_[vid](2);
                                auto nx = vc_.normals_[vid](0);
                                auto ny = vc_.normals_[vid](1);
                                auto nz = vc_.normals_[vid](2);
                                auto r = vc_.colors_[vid](0);
                                auto g = vc_.colors_[vid](1);
                                auto b = vc_.colors_[vid](2);
                                std::cout << "vxl " << vid  << " : [" <<  x << " " << y << " " << z << "] : [" <<  nx << " " << ny << " " << nz << "]" << "] : [" <<  r << " " << g << " " << b << "]" << std::endl;
                            }
                        }
                    }
                }

                depth++;

                if (!leaf_appended) {
                    break;
                }
            }

            // update label_ and property
            {
                vlabel_ = v2l;
                for (unsigned int i=0; i<svxls_.size(); i++) {
                    if (sv2p[i].num_of_points_ == 0) {
                        svxls_[i]->prop_ = RegionProperty();
                        svxls_[i]->seed_ = NO_VOXEL;
                    } else {
                        svxls_[i]->prop_ = sv2p[i];
                    }
                }
            }

            LabelAllVoxels(r_seed);
        }

        ComputeSuperVoxelAdjacency(r_adj);
    }

    void Label(VoxelID seed, SuperVoxelID svid, int r_seed)
    {
        auto f = [this, svid] (const VoxelID &v) -> bool {
            if (vlabel_[v] == NO_LABEL) {
                vlabel_[v] = svid;
                svxls_[svid]->prop_ = svxls_[svid]->prop_ + MakeRegionProperty(v);
                return true;
            }
            return false;
        };
        BFS<VoxelID>(seed, vadj_, r_seed, f);
    }

    void LabelAllVoxels(int r_seed)
    {
       for (unsigned int i=0; i<vlabel_.size(); i++) {
           if (vlabel_[i] == NO_LABEL) {
                SuperVoxelID freshID = svxls_.size();
                auto svxl = std::make_shared<SuperVoxel>();
                svxls_.push_back(svxl);
                Label(i, freshID, r_seed);
           }
       }
    }

    void ComputeSuperVoxelAdjacency(double r_adj)
    {
        auto vlabel = vlabel_;

        ASSERT( vlabel_.size() == vadj_.size());

        for(unsigned int i=0; i<rvc_.points_.size(); i++) {
            std::vector<int> indices;
            std::vector<double> distance2;
            int k = kdt_.SearchKNN(rvc_.points_[i], 1, indices, distance2);
            vlabel.push_back(vlabel[indices[0]]);
        }

        for (unsigned int vid=0; vid<rvadj_.size(); vid++) {
            SuperVoxelID svid = vlabel[vid];
            if (svid == NO_LABEL) {
                continue;
            }
            ASSERT(svxls_[svid]->seed_ != NO_VOXEL);
            for (auto nvid :rvadj_[vid]) {
                SuperVoxelID nsvid = vlabel[nvid];
                if (nsvid == NO_LABEL) {
                    continue;
                }
                ASSERT(svxls_[nsvid]->seed_ != NO_VOXEL);
                //std::cout << vid << "[" << svid << "] -> " << nvid << "[" << nsvid << "]" << std::endl;
                if (svid != nsvid) {
                    //std::cout << svid << " -> " << nsvid  << std::endl;
                    svadj_[svid].insert(nsvid);
                }
            }
        }

    }

public:
    //super voxel
    KDTreeFlann kdt_;
    PointCloud vc_; // voxel cloud
    PointCloud rvc_; // debug : removed voxel cloud
    std::vector<std::set<VoxelID> > vadj_; // voxel -> neighbor voxels(only stable normal voxel)
    std::vector<std::set<VoxelID> > rvadj_; // voxel -> neighbor voxels
    std::vector<std::shared_ptr<SuperVoxel> > svxls_;
    std::vector<SuperVoxelID> vlabel_; // voxelid -> supervoxel
    std::map<SuperVoxelID, std::set<SuperVoxelID> > svadj_; // supervoxel -> neighbor supervoxels

	std::vector<RegionProperty> mergedsvxls_;
};

}  // namespace geometry
}  // namespace open3d


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [oneminus]
#include <boost/simd/arithmetic.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft = bs::pack <float, 4>;
using pack_it = bs::pack <std::uint16_t,4>;

int main()
{
  pack_ft pf = {-1.0f, 2.0f, -3.0f, -32768.0f};
  pack_it pi = { 0,   -1,    2,    3 };

  std::cout
    << "---- simd" << '\n'
    << "<- pf =                                " << pf << '\n'
    << "-> bs::oneminus(pf) =                  " << bs::oneminus(pf) << '\n'
    << "<- pi =                                " << pi << '\n'
    << "-> bs::oneminus(pi) =                  " << bs::oneminus(pi) << '\n'
    << "-> bs::saturated_(bs::oneminus(pi)) =  " << bs::saturated_(bs::oneminus)(pi) << '\n';

  float xf = -327.0f;
  std::uint16_t xi =  2;

  std::cout
    << "---- scalar"  << '\n'
    << "<- xf =                                " << xf << '\n'
    << "-> bs::oneminus(xf) =                  " << bs::oneminus(xf) << '\n'
    << "<- xi =                                " << xi << '\n'
    << "-> bs::oneminus(xi) =                  " << bs::oneminus(xi) << '\n'
    << "-> bs::saturated_(bs::oneminus(xi)) =  " << bs::saturated_(bs::oneminus)(xi) << '\n';
  return 0;
}
//! [oneminus]


#pragma once
#include "criteria.hpp"
#include "dijkstra.hpp"
#include "graph.hpp"
#include "stringy_enum.hpp"
#include <boost/heap/pairing_heap.hpp>
#include <unordered_set>

namespace sssp {

// Implements Crauser's IN criteria. Additionally instead of using the minimal edges,
// with dynamic=true, one can use the minimal non-settled edge.
class crauser_in : public criteria {
  public:
    crauser_in(const sssp::graph* graph, size_t start_node, bool dynamic);
    virtual void relaxable_nodes(todo_output& output) const override;
    virtual void changed_predecessor(size_t node, size_t predecessor, double distance) override;
    virtual void relaxed_node(size_t node) override;
    virtual bool is_complete() const override { return true; }
    bool dynamic() const { return m_dynamic; }

  private:
    struct node_info;
    struct node_info_compare_distance {
        bool operator()(const node_info* a, const node_info* b) const;
    };
    // threshold(n) = tentative(n) - min{ cost of incmoing edges (not settled if dynamic) }
    // Called "i" in the paper.
    struct node_info_compare_threshold {
        bool operator()(const node_info* a, const node_info* b) const;
    };

    using distance_queue = boost::heap::pairing_heap<node_info*, boost::heap::compare<node_info_compare_distance>>;
    using threshold_queue = boost::heap::pairing_heap<node_info*, boost::heap::compare<node_info_compare_threshold>>;

    struct node_info {
        node_info(const sssp::graph& g, size_t index);
        size_t index;
        std::vector<edge_info> incoming;
        double tentative_distance = INFINITY;
        bool settled = false;
        distance_queue::handle_type distance_queue_handle;
        threshold_queue::handle_type threshold_queue_handle;

        double threshold() const;
    };

    bool m_dynamic;
    node_map<node_info> m_node_info;
    distance_queue m_distance_queue;
    threshold_queue m_threshold_queue;
};

// Implements Crauser's OUT criteria. Additionally instead of using the minimal edges,
// with dynamic=true, one can use the minimal non-settled edge.
class crauser_out : public criteria {
  public:
    crauser_out(const sssp::graph* graph, size_t start_node, bool dynamic);
    virtual void relaxable_nodes(todo_output& output) const override;
    virtual void changed_predecessor(size_t node, size_t predecessor, double distance) override;
    virtual void relaxed_node(size_t node) override;
    virtual bool is_complete() const override { return true; }
    bool dynamic() const { return m_dynamic; }

  private:
    struct node_info;
    struct node_info_compare_distance {
        bool operator()(const node_info* a, const node_info* b) const;
    };
    // threshold(n) = tentative(n) + min{ outgoing edge (not settled if dynamic) }
    // Called "L" in the paper (to be exact this are the values of all nodes, not the final L)
    struct node_info_compare_threshold {
        bool operator()(const node_info* a, const node_info* b) const;
    };

    using distance_queue = boost::heap::pairing_heap<node_info*, boost::heap::compare<node_info_compare_distance>>;
    using threshold_queue = boost::heap::pairing_heap<node_info*, boost::heap::compare<node_info_compare_threshold>>;

    struct node_info {
        node_info(const sssp::graph& g, size_t index);
        size_t index;
        std::vector<edge_info> outgoing;
        double tentative_distance = INFINITY;
        bool settled = false;
        distance_queue::handle_type distance_queue_handle;
        threshold_queue::handle_type threshold_queue_handle;

        double threshold() const;
    };

    bool m_dynamic;
    node_map<node_info> m_node_info;
    distance_queue m_distance_queue;
    threshold_queue m_threshold_queue;
};

} // namespace sssp