#define BOOST_TEST_NO_LIB
#define BOOST_TEST_MAIN
#include <fstream>
#include <gp/tree/tree.hpp>
#include <gp/node/node.hpp>
#include <gp/tree/io.hpp>
#include <boost/test/unit_test.hpp>
using namespace gp;

constexpr std::size_t MAX_SUBROUTINE_ARG_NUM = 3;

using TypeList = std::tuple<int>;

BOOST_AUTO_TEST_SUITE(io_test)
    BOOST_AUTO_TEST_CASE(io_test) {
        utility::StringToType stringToType;
        stringToType.setTypeNamePair<int>("int");
        stringToType.setTypeNamePair<utility::LeftHandValue<int>>("lvalue[int]");
        stringToType.setTypeNamePair<utility::Reference<int>>("ref[int]");
        stringToType.setTypeNamePair<bool>("bool");

        tree::TreeIO<MAX_SUBROUTINE_ARG_NUM, TypeList> treeIO;

        treeIO.registerNode(std::make_unique<node::PrognNode<int,2>>());
        treeIO.registerNode(std::make_unique<node::AddNode<int>>());
        treeIO.registerNode(std::make_unique<node::SubstitutionNode<int>>());
        treeIO.registerNode(std::make_unique<node::LocalVariableNode<int>>());
        treeIO.registerNode(std::make_unique<node::LocalVariableNode<utility::LeftHandValue<int>>>());
        treeIO.registerNode(std::make_unique<node::ArgumentNode<int,0>>());
        treeIO.registerNode(std::make_unique<node::ArgumentNode<int,1>>());
        treeIO.registerNode(std::make_unique<node::ArgumentNode<utility::Reference<int>, 0>>());
        treeIO.registerNode(std::make_unique<node::ArgumentNode<utility::LeftHandValue<int>, 0>>());
        treeIO.registerNode(std::make_unique<node::ConstNode<int>>(0));
        treeIO.registerNode(std::make_unique<node::GreaterNode<int>>());
        treeIO.registerNode(std::make_unique<node::GreaterEqNode<int>>());
        treeIO.registerNode(std::make_unique<node::LessThanEqNode<int>>());
        treeIO.registerNode(std::make_unique<node::LessThanNode<int>>());
        treeIO.registerNode(std::make_unique<node::EqualNode<int>>());
        treeIO.registerNode(std::make_unique<node::NotEqualNode<int>>());
        treeIO.registerNode(std::make_unique<node::IfNode<int>>());
        treeIO.registerNode(std::make_unique<node::MultiplyNode<int>>());
        treeIO.registerNode(std::make_unique<node::SubtractNode<int>>());
        treeIO.registerNode(std::make_unique<node::ConstNode<bool>>(false));
        treeIO.registerNode(std::make_unique<node::NopNode<int>>());
        treeIO.registerNode(std::make_unique<node::NopNode<bool>>());

        auto var0 = node::NodeInterface::createInstance<node::LocalVariableNode<int>>(0);
        auto arg1 = node::NodeInterface::createInstance<node::ArgumentNode<int, 1>>();
        auto add1 = node::NodeInterface::createInstance<node::AddNode<int>>();

        add1->setChild(0, std::move(arg1));
        add1->setChild(1, std::move(var0));

        auto c1 = node::NodeInterface::createInstance<node::ConstNode<int>>(10);
        auto add2 = node::NodeInterface::createInstance<node::AddNode<int>>();

        add2->setChild(0, std::move(c1));
        add2->setChild(1, std::move(add1));

        auto lvar0 = node::NodeInterface::createInstance<node::LocalVariableNode<utility::LeftHandValue<int>>>(0);
        auto arg0 = node::NodeInterface::createInstance<node::ArgumentNode<int, 0>>();
        auto subst = node::NodeInterface::createInstance<node::SubstitutionNode<int>>();

        subst->setChild(0, std::move(lvar0));
        subst->setChild(1, std::move(arg0));

        auto progn = node::NodeInterface::createInstance<node::PrognNode<int, 2>>();

        progn->setChild(0, std::move(subst));
        progn->setChild(1, std::move(add2));

        auto treeProperty = tree::TreeProperty{&utility::typeInfo<int>(),
                                               {&utility::typeInfo<int>(), &utility::typeInfo<int>()},
                                               {&utility::typeInfo<int>()},
                                               "TestTree"};

        tree::Tree tree(std::move(treeProperty), std::move(progn));

        for(int i = -10; i < 10; ++i) {
            int a0 = i;
            int a1 = 2 * i;
            auto ans = tree.evaluate(std::make_tuple(a0, a1));
            BOOST_CHECK_EQUAL(static_cast<int>(ans.getEvaluationStatus()), static_cast<int>(utility::EvaluationStatus::ValueReturned));
            BOOST_CHECK_EQUAL(std::any_cast<int>(ans.getReturnValue()), 10 + a0 + a1);
        }

        std::ofstream fout("TestTree.xml");
        treeIO.writeTree(tree, fout);
        fout.close();

        std::ifstream fin("TestTree.xml");
        auto inTreeResult = treeIO.readTree(fin, stringToType);
        BOOST_CHECK(inTreeResult);
        auto& inTree = inTreeResult.unwrap();
        BOOST_CHECK_EQUAL(inTree.getName(), tree.getName());
        BOOST_CHECK_EQUAL(inTree.getArgumentNum(), tree.getArgumentNum());
        BOOST_CHECK_EQUAL(inTree.getLocalVariableNum(), tree.getLocalVariableNum());
        BOOST_CHECK(tree.getReturnType() == inTree.getReturnType());
        for(int i = 0; i < tree.getArgumentNum(); ++i) {
            BOOST_CHECK(tree.getArgumentType(i) == inTree.getArgumentType(i));
        }
        for(int i = 0; i < tree.getLocalVariableNum(); ++i) {
            BOOST_CHECK(tree.getLocalVariableType(i) == inTree.getLocalVariableType(i));
        }

        for(int i = -10; i < 10; ++i) {
            int a0 = i;
            int a1 = 2 * i;
            auto ans = inTree.evaluate(std::make_tuple(a0, a1));
            BOOST_CHECK_EQUAL(static_cast<int>(ans.getEvaluationStatus()), static_cast<int>(utility::EvaluationStatus::ValueReturned));
            BOOST_CHECK_EQUAL(std::any_cast<int>(ans.getReturnValue()), 10 + a0 + a1);
        }
    }
    BOOST_AUTO_TEST_CASE(recursion_test) {
        utility::StringToType stringToType;
        stringToType.setTypeNamePair<int>("int");
        stringToType.setTypeNamePair<utility::LeftHandValue<int>>("lvalue[int]");
        stringToType.setTypeNamePair<utility::Reference<int>>("ref[int]");
        stringToType.setTypeNamePair<bool>("bool");

        tree::TreeIO<MAX_SUBROUTINE_ARG_NUM, TypeList> treeIO;

        treeIO.registerNode(std::make_unique<node::PrognNode<int,2>>());
        treeIO.registerNode(std::make_unique<node::AddNode<int>>());
        treeIO.registerNode(std::make_unique<node::SubstitutionNode<int>>());
        treeIO.registerNode(std::make_unique<node::LocalVariableNode<int>>());
        treeIO.registerNode(std::make_unique<node::LocalVariableNode<utility::LeftHandValue<int>>>());
        treeIO.registerNode(std::make_unique<node::ArgumentNode<int,0>>());
        treeIO.registerNode(std::make_unique<node::ArgumentNode<int,1>>());
        treeIO.registerNode(std::make_unique<node::ArgumentNode<utility::Reference<int>, 0>>());
        treeIO.registerNode(std::make_unique<node::ArgumentNode<utility::LeftHandValue<int>, 0>>());
        treeIO.registerNode(std::make_unique<node::ConstNode<int>>(0));
        treeIO.registerNode(std::make_unique<node::GreaterNode<int>>());
        treeIO.registerNode(std::make_unique<node::GreaterEqNode<int>>());
        treeIO.registerNode(std::make_unique<node::LessThanEqNode<int>>());
        treeIO.registerNode(std::make_unique<node::LessThanNode<int>>());
        treeIO.registerNode(std::make_unique<node::EqualNode<int>>());
        treeIO.registerNode(std::make_unique<node::NotEqualNode<int>>());
        treeIO.registerNode(std::make_unique<node::IfNode<int>>());
        treeIO.registerNode(std::make_unique<node::MultiplyNode<int>>());
        treeIO.registerNode(std::make_unique<node::SubtractNode<int>>());
        treeIO.registerNode(std::make_unique<node::ConstNode<bool>>(false));
        treeIO.registerNode(std::make_unique<node::NopNode<int>>());
        treeIO.registerNode(std::make_unique<node::NopNode<bool>>());

        std::stringstream recursionSubTreeStr(std::string("<?xml version=\"1.0\" encoding=\"utf-8\"?>\n")
                + "<tree>\n"
                + "    <name>Fact[int]</name>\n"
                + "    <return_type>int</return_type>\n"
                + "    <arguments>\n"
                + "        <type>int</type>\n"
                + "    </arguments>\n"
                + "    <local_variables>\n"
                + "    </local_variables>\n"
                + "    <tree_entity>\n"
                + "        +--If[int]\n"
                + "            |\n"
                + "            +--Greater[int]\n"
                + "            |   |\n"
                + "            |   +--Argument[int,0]\n"
                + "            |   |\n"
                + "            |   +--Const[int,1]\n"
                + "            |\n"
                + "            +--Mult[int]\n"
                + "            |   |\n"
                + "            |   +--Argument[int,0]\n"
                + "            |   |\n"
                + "            |   +--Fact[int]\n"
                + "            |       |\n"
                + "            |       +--Sub[int]\n"
                + "            |           |\n"
                + "            |           +--Argument[int,0]\n"
                + "            |           |\n"
                + "            |           +--Const[int,1]\n"
                + "            |\n"
                + "            +--Const[int,1]\n"
                + "    </tree_entity>\n"
                + "</tree>");

        auto subroutineResult = treeIO.loadSubroutine(recursionSubTreeStr, stringToType);
        BOOST_CHECK(subroutineResult);
        auto [factNode, factSubroutineProperty] = std::move(subroutineResult).unwrap();
        BOOST_CHECK(*factSubroutineProperty.returnType == utility::typeInfo<int>());
        BOOST_CHECK_EQUAL(factSubroutineProperty.argumentTypes.size(), 1);
        BOOST_CHECK(*factSubroutineProperty.argumentTypes[0] == utility::typeInfo<int>());
        BOOST_CHECK_EQUAL(factSubroutineProperty.localVariableTypes.size(), 0);
        BOOST_CHECK_EQUAL(factSubroutineProperty.name, "Fact[int]");

        factNode->setChild(0, node::NodeInterface::createInstance<node::ArgumentNode<int, 0>>());
        tree::Tree tree(std::move(factSubroutineProperty), std::move(factNode));

        int acc = 1;
        for(int i = 0; i < 10; ++i) {
            auto ans = tree.evaluate(std::make_tuple(i));
            BOOST_CHECK(ans.getEvaluationStatus() == utility::EvaluationStatus::ValueReturned);
            BOOST_CHECK_EQUAL(std::any_cast<int>(ans.getReturnValue()), acc);
            acc *= i + 1;
        }
    }
    BOOST_AUTO_TEST_CASE(reference_test) {
        utility::StringToType stringToType;
        stringToType.setTypeNamePair<int>("int");
        stringToType.setTypeNamePair<utility::LeftHandValue<int>>("lvalue[int]");
        stringToType.setTypeNamePair<utility::Reference<int>>("ref[int]");
        stringToType.setTypeNamePair<bool>("bool");

        tree::TreeIO<MAX_SUBROUTINE_ARG_NUM, TypeList> treeIO;

        treeIO.registerNode(std::make_unique<node::PrognNode<int,2>>());
        treeIO.registerNode(std::make_unique<node::AddNode<int>>());
        treeIO.registerNode(std::make_unique<node::SubstitutionNode<int>>());
        treeIO.registerNode(std::make_unique<node::LocalVariableNode<int>>());
        treeIO.registerNode(std::make_unique<node::LocalVariableNode<utility::LeftHandValue<int>>>());
        treeIO.registerNode(std::make_unique<node::ArgumentNode<int,0>>());
        treeIO.registerNode(std::make_unique<node::ArgumentNode<int,1>>());
        treeIO.registerNode(std::make_unique<node::ArgumentNode<utility::Reference<int>, 0>>());
        treeIO.registerNode(std::make_unique<node::ArgumentNode<utility::LeftHandValue<int>, 0>>());
        treeIO.registerNode(std::make_unique<node::ConstNode<int>>(0));
        treeIO.registerNode(std::make_unique<node::GreaterNode<int>>());
        treeIO.registerNode(std::make_unique<node::GreaterEqNode<int>>());
        treeIO.registerNode(std::make_unique<node::LessThanEqNode<int>>());
        treeIO.registerNode(std::make_unique<node::LessThanNode<int>>());
        treeIO.registerNode(std::make_unique<node::EqualNode<int>>());
        treeIO.registerNode(std::make_unique<node::NotEqualNode<int>>());
        treeIO.registerNode(std::make_unique<node::IfNode<int>>());
        treeIO.registerNode(std::make_unique<node::MultiplyNode<int>>());
        treeIO.registerNode(std::make_unique<node::SubtractNode<int>>());
        treeIO.registerNode(std::make_unique<node::ConstNode<bool>>(false));
        treeIO.registerNode(std::make_unique<node::NopNode<int>>());
        treeIO.registerNode(std::make_unique<node::NopNode<bool>>());
        treeIO.registerNode(std::make_unique<node::NopNode<utility::Reference<int>>>());

        std::stringstream referenceTestSubStr(std::string("<?xml version=\"1.0\" encoding=\"utf-8\"?>\n")
                                                      + "<tree>\n"
                                                      + "    <name>ReferenceTestSub</name>\n"
                                                      + "    <return_type>int</return_type>\n"
                                                      + "    <arguments>\n"
                                                      + "        <type>ref[int]</type>\n"
                                                      + "    </arguments>\n"
                                                      + "    <local_variables>\n"
                                                      + "    </local_variables>\n"
                                                      + "    <tree_entity>\n"
                                                      + "        +--Substitute[int]\n"
                                                      + "             |\n"
                                                      + "             +--Argument[lvalue[int],0]\n"
                                                      + "             |\n"
                                                      + "             +--Add[int]\n"
                                                      + "                 |\n"
                                                      + "                 +--Const[int,10]\n"
                                                      + "                 |\n"
                                                      + "                 +--Argument[int,0]\n"
                                                      + "    </tree_entity>\n"
                                                      + "</tree>");

        auto subroutineResult = treeIO.loadSubroutine(referenceTestSubStr, stringToType);
        BOOST_CHECK(subroutineResult);
        auto [refereneTestSub, referenceTestSubProperty] = std::move(subroutineResult).unwrap();
        BOOST_CHECK(*referenceTestSubProperty.returnType == utility::typeInfo<int>());
        BOOST_CHECK_EQUAL(referenceTestSubProperty.argumentTypes.size(), 1);
        BOOST_CHECK(*referenceTestSubProperty.argumentTypes[0] == utility::typeInfo<utility::Reference<int>>());
        BOOST_CHECK_EQUAL(referenceTestSubProperty.localVariableTypes.size(), 0);
        BOOST_CHECK_EQUAL(referenceTestSubProperty.name, "ReferenceTestSub");

        {
            std::stringstream referenceTestStr(std::string("<?xml version=\"1.0\" encoding=\"utf-8\"?>\n")
                                               + "<tree>\n"
                                               + "    <name>ReferenceTest</name>\n"
                                               + "    <return_type>int</return_type>\n"
                                               + "    <arguments>\n"
                                               + "        <type>int</type>\n"
                                               + "    </arguments>\n"
                                               + "    <local_variables>\n"
                                               + "    </local_variables>\n"
                                               + "    <tree_entity>\n"
                                               + "    +--Progn[int,2]\n"
                                               + "        |\n"
                                               + "        +--ReferenceTestSub\n"
                                               + "        |   |\n"
                                               + "        |   +--Argument[ref[int],0]\n"
                                               + "        |\n"
                                               + "        +--Argument[int,0]\n"
                                               + "    </tree_entity>\n"
                                               + "</tree>");

            auto treeResult = treeIO.readTree(referenceTestStr, stringToType);
            BOOST_CHECK(treeResult);
            auto& tree = treeResult.unwrap();

            for (int i = 0; i < 10; ++i) {
                auto ans = tree.evaluate(std::make_tuple(i));
                BOOST_CHECK(ans.getEvaluationStatus() == utility::EvaluationStatus::ValueReturned);
                BOOST_CHECK_EQUAL(std::any_cast<int>(ans.getReturnValue()), i + 10);
            }
        }
        {//illeagal reference is passed to the subroutine node
            std::stringstream referenceTestStr(std::string("<?xml version=\"1.0\" encoding=\"utf-8\"?>\n")
                                               + "<tree>\n"
                                               + "    <name>ReferenceTest</name>\n"
                                               + "    <return_type>int</return_type>\n"
                                               + "    <arguments>\n"
                                               + "        <type>int</type>\n"
                                               + "    </arguments>\n"
                                               + "    <local_variables>\n"
                                               + "    </local_variables>\n"
                                               + "    <tree_entity>\n"
                                               + "    +--Progn[int,2]\n"
                                               + "        |\n"
                                               + "        +--ReferenceTestSub\n"
                                               + "        |   |\n"
                                               + "        |   +--Nop[ref[int]]\n"
                                               + "        |\n"
                                               + "        +--Argument[int,0]\n"
                                               + "    </tree_entity>\n"
                                               + "</tree>");

            auto treeResult = treeIO.readTree(referenceTestStr, stringToType);
            BOOST_CHECK(treeResult);
            auto& tree = treeResult.unwrap();

            for (int i = 0; i < 10; ++i) {
                auto ans = tree.evaluate(std::make_tuple(i));
                BOOST_CHECK(ans.getEvaluationStatus() == utility::EvaluationStatus::InvalidValue);
            }
        }
    }
BOOST_AUTO_TEST_SUITE_END()


#ifndef BOOST_MPL_FIXED_C_HPP_INCLUDED
#define BOOST_MPL_FIXED_C_HPP_INCLUDED

// Copyright Aleksey Gurtovoy 2000-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Source: /cvsroot/boost/boost/boost/mpl/math/fixed_c.hpp,v $
// $Date: 2004/09/02 15:41:01 $
// $Revision: 1.4 $

#include <boost/mpl/aux_/config/static_constant.hpp>

namespace boost { namespace mpl {

template<
      long IntegerPart
    , unsigned long FractionPart
    >
struct fixed_c
{
    BOOST_STATIC_CONSTANT(long, integer_part = IntegerPart);
    BOOST_STATIC_CONSTANT(unsigned long, fraction_part = FractionPart);
    typedef fixed_c<IntegerPart, FractionPart> type;

    fixed_c() {}
};

}}

#endif // BOOST_MPL_FIXED_C_HPP_INCLUDED


#include <algorithm>
#include <iostream>
#include <iterator>
#include <sstream>
#include <string>
#include <vector>

#include <boost/range/irange.hpp>

#include "input.hpp"

using uintmax = std::uintmax_t;

struct Reindeer {
  uintmax speed,
          runtime,
          resttime,
          total_distance = 0,
          score = 0;
};

auto& operator>>(std::istream& in, Reindeer& r) {
  std::string s; // throwaway input
  return in >> s >> s >> s
            >> r.speed >> s >> s
            >> r.runtime >> s >> s >> s >> s >> s >> s
            >> r.resttime >> s;
}

using Reindeers = std::vector<Reindeer>;

auto parse(const std::string& input) {
  auto stream = std::stringstream{input};

  using iterator = std::istream_iterator<Reindeer>;

  const auto stream_begin = iterator{stream};
  const auto stream_end   = iterator{};

  return Reindeers{stream_begin, stream_end};
}

auto calculate_scores(Reindeers& reindeers, uintmax seconds) {

  const auto cmp_distance = [] (const auto& a, const auto& b) {
    return a.total_distance < b.total_distance;
  };

  const auto timespan = boost::irange({1}, (seconds + 1));

  for(const auto second : timespan) {

    for(auto& reindeer : reindeers) {

      const auto runtime = reindeer.runtime;
      const auto cycle = (runtime + reindeer.resttime);
      const auto time_left = (second % cycle);

      // check if reindeer is running right this second
      const auto has_time_left = (time_left > 0) && (time_left <= runtime);

      if(has_time_left) {
        reindeer.total_distance += reindeer.speed;
      }
    }

    const auto leading_distance = std::max_element(reindeers.begin(), reindeers.end(), cmp_distance)->total_distance;

    // one point for each leader
    for(auto& reindeer : reindeers) {
      reindeer.score += (reindeer.total_distance == leading_distance);
    }
  }
}

auto max_score(const Reindeers& reindeers) {

  const auto cmp_score  = [] (const auto& a, const auto& b) {
    return a.score < b.score;
  };

  return std::max_element(reindeers.begin(), reindeers.end(), cmp_score)->score;
}

auto solution(const std::string& input, uintmax seconds) {

  auto reindeers = parse(input);

  calculate_scores(reindeers, seconds);

  return max_score(reindeers);
}

int main() {

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

}


/*
Copyright (C) 2012 Mathias Eitz and Ronald Richter.
All rights reserved.

This file is part of the imdb library and is made available under
the terms of the BSD license (see the LICENSE file).
*/

#ifndef KMEANS_HPP
#define KMEANS_HPP

#include <vector>
#include <algorithm>
#include <iostream>
#include <set>

#include <boost/random.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/locks.hpp>

#include <QTime>

#include "../search/distance.hpp"
#include "kmeans_init.hpp"

/**
 * @brief Standard kmeans clustering
 */
template <class collection_t, class dist_fn>
class kmeans
{
    typedef boost::mutex               mutex_t;
    typedef boost::lock_guard<mutex_t> locker_t;

    typedef typename collection_t::value_type sample_t;

    public:

    /**
     * @brief Standard k-means clustering given a distance function
     * @param collection Datastructure containing the samples to be clustered, typically a vector<vector<float> >, where the 'inner'
     * vector<float> would be a single sample.
     * @param numclusters Number of clusters to use.
     * @param initalgorithm Algorithm used to estimate the inital cluster centers
     * @param distfn Distance function used for comparing two samples.
     */
    kmeans(const collection_t& collection, std::size_t numclusters, KmeansInitAlgorithm initalgorithm = KmeansInitRandom, const dist_fn& distfn = dist_fn())
     : _collection(collection), _distfn(distfn), _centers(numclusters), _clusters(collection.size())
    {
        // get initial centers
        std::vector<std::size_t> initindices;
        if (initalgorithm == KmeansInitPlusPlus)
        {
            kmeans_init_plusplus(initindices, collection, numclusters, distfn);
        }
        else
        {
            kmeans_init_random(initindices, collection, numclusters);
        }

        for (std::size_t i = 0; i < initindices.size(); i++) _centers[i] = collection[initindices[i]];
    }

    /**
     * @brief Perform k-means clustering on the dataset provided in the constructor
     *
     * Iterates until at least one of the following two criteria are met:
     * - maximum number of iterations reached
     * - the fraction of samples that changed clusters is below minchangesfraction
     * @param maxiteration Maximum number of iterations
     * @param minchangesfraction Fraction of changes, if less changes happen in a certain iteration, clustering is done.
     */
    void run(std::size_t maxiteration, double minchangesfraction)
    {
        using namespace boost;

        // main iteration
        std::size_t iteration = 0;
        for (;;)
        {
            if (maxiteration > 0 && iteration == maxiteration) break;

            QTime time;
            time.start();
            std::size_t changes = 0;

            // distribute items on clusters in parallel
            thread_group pool;
            std::size_t idx = 0;
            mutex_t mtx;
            for (std::size_t i = 0; i < thread::hardware_concurrency(); i++)
            {
                pool.create_thread(bind(&kmeans::distribute_samples, this, ref(idx), ref(changes), ref(mtx)));
            }
            pool.join_all();

            iteration++;

            std::cout << "changes: " << changes << " distribution time: " << time.elapsed() << std::endl;

            if (changes <= std::ceil(_collection.size() * minchangesfraction)) break;

            // compute new centers
            std::vector<std::size_t> clustersize(_centers.size(), 0);
            for (std::size_t i = 0; i < _collection.size(); i++)
            {
                std::size_t k = _clusters[i];

                // if it is the first assignment for that cluster, then zero the center
                if (clustersize[k] == 0) std::fill(_centers[k].begin(), _centers[k].end(), 0);

                add_operation(_centers[k], _collection[i]);
                clustersize[k]++;
            }

            // assign new centers
            std::vector<std::size_t> invalid, valid;
            for (std::size_t i = 0; i < _centers.size(); i++)
            {
                if (clustersize[i] > 0)
                {
                    div_operation(_centers[i], clustersize[i]);
                    valid.push_back(i);
                }
                else
                {
                    invalid.push_back(i);
                }
                //std::cout << "clustersize " << i << ": " << clustersize[i] << std::endl;
            }

            // fix invalid centers, i.e. those with no members
            while (!invalid.empty() && !valid.empty())
            {
                std::size_t current = invalid.back();
std::cout << "handle invalid clusters: " << invalid.size() << std::endl;
                // compute for each valid cluster the variance
                // of distances to all members and get the
                // most distant member
                std::vector<double> maxdist(_centers.size(), 0.0);
                std::vector<double> variance(_centers.size(), 0.0);
                std::vector<std::size_t> farthest(_centers.size());
                for (std::size_t i = 0; i < valid.size(); i++)
                {
                    std::size_t c = valid[i];

                    for (std::size_t k = 0; k < _collection.size(); k++)
                    {
                        if (_clusters[k] != c) continue;

                        double d = _distfn(_collection[k], _centers[c]);
                        if (d > maxdist[c])
                        {
                            maxdist[c] = d;
                            farthest[c] = k;
                        }
                        variance[c] += d*d;
                    }
                    variance[c] /= clustersize[c];
                }

                // get cluster with highest variance and make
                // the farthest member of that cluster the
                // new center
                std::size_t c = std::distance(variance.begin(), std::max_element(variance.begin(), variance.end()));
                _centers[current] = _collection[farthest[c]];
                _clusters[farthest[c]] = current;

std::cout << "reassign " << current << " to sample " << farthest[c] << " of cluster " << c << std::endl;

                valid.pop_back();
                invalid.pop_back();
            }

std::cout << "iteration " << iteration << " time: " << time.elapsed() << std::endl;
        }

std::cout << "kmeans iterations: " << iteration << std::endl;
    }

    /// Run clustering, using theoretically unlimited number of iterations. Clustering will
    /// stop when the fraction of changes falls below 0.01
    void run_default()
    {
        this->run(std::numeric_limits<std::size_t>::max(), 0.01);
    }

    /// Vector of cluster membership: clusters[i] = j means that the sample with index i
    /// is a member of cluster j
    const std::vector<std::size_t>& clusters() const
    {
        return _clusters;
    }

    /// Vector of cluster centers
    const std::vector<sample_t>& centers() const
    {
        return _centers;
    }

    /// Convenience function generating a clustering table:
    /// table[i][j] = k means that the sample with index k belongs to cluster i.
    template <class index_t>
    void make_cluster_table(std::vector<std::vector<index_t> >& table)
    {
        table.resize(_centers.size());
        for (std::size_t i = 0; i < _clusters.size(); i++) table[_clusters[i]].push_back(i);
    }

    template <class T>
    static void add_operation(T& lhs, const T& rhs)
    {
        for (std::size_t i = 0; i < lhs.size(); i++) lhs[i] += rhs[i];
    }

    template <class T>
    static void div_operation(T& lhs, double rhs)
    {
        for (std::size_t i = 0; i < lhs.size(); i++) lhs[i] /= rhs;
    }

    private:

    void distribute_samples(std::size_t& index, std::size_t& changes, mutex_t& mutex)
    {
        std::vector<double> dists(_centers.size());
        std::size_t currentchanges = 0;

        for (;;)
        {
            std::size_t i;

            {
                locker_t locker(mutex);
                if (index == _collection.size()) break;
                i = index++;
            }

            // compute distance of current point to every center
            std::transform(_centers.begin(), _centers.end(), dists.begin(), boost::bind(_distfn, boost::ref(_collection[i]), boost::arg<1>()));

            // find the minimum distance, i.e. the nearest center
            std::size_t c = std::distance(dists.begin(), std::min_element(dists.begin(), dists.end()));

            // update cluster membership
            {
                locker_t locker(_mutex);

                if (_clusters[i] != c)
                {
                    _clusters[i] = c;
                    currentchanges++;
                }
//if (i % 1000 == 0) std::cout << "kmeans distribute: " << i << std::endl;
            }
        }

        {
            locker_t locker(mutex);
            changes += currentchanges;
        }
    }

    const collection_t& _collection;
    const dist_fn&      _distfn;

    std::vector<sample_t>    _centers;
    std::vector<std::size_t> _clusters;

    boost::mutex        _mutex;
};

#endif // KMEANS_HPP


/***************************************************************
 * Author:    Yixin Zhuang (yixin.zhuang@gmail.com)
 **************************************************************/

#include "core/Segmentation.h"

#include <fstream>
#include <queue>

//i use boost_1_67_0
#include <boost/config.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
using namespace boost;
//boost graph
typedef adjacency_list < vecS, vecS, undirectedS,
	no_property, property < edge_weight_t, double > > graph_t;
typedef graph_traits < graph_t >::vertex_descriptor vertex_descriptor;
typedef graph_traits < graph_t >::edge_descriptor edge_descriptor;
typedef std::pair<int, int> Edge;

using namespace GeoProperty;

#include "andres/graph/graph.hxx"
#include "andres/graph/complete-graph.hxx"
#include "andres/graph/multicut-lifted/greedy-additive.hxx"
#include "andres/graph/multicut-lifted/kernighan-lin.hxx"

void Mitani_WaterShed::distanceToFeature_ShortestPath(std::list<MyMesh::Vertex>& vers, std::list<MyMesh::Edge>& edges, std::set<int>& sources, std::vector<double>& res)
{
	//build dual graph;

	int superNode = vers.size();
	int vNum = vers.size() + 1; //will add a super node
	int eNum = edges.size() + sources.size();//will add edges connecting super node and sources

	// init graph
	graph_t g(vNum);
	property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
	for (auto i = edges.begin(); i != edges.end(); i++)
	{
		int nodeID[] = { i->vertex_iter(0)->id(), i->vertex_iter(1)->id() };
		graph_traits < graph_t >::edge_descriptor e;
		bool inserted;
		boost::tie(e, inserted) = add_edge(nodeID[0], nodeID[1], g);
		if (!inserted) cout << "insert edge failed" << endl;
		weightmap[e] = i->length();
	}
	for (auto i = sources.begin(); i != sources.end(); i++)
	{
		graph_traits < graph_t >::edge_descriptor e;
		bool inserted;
		boost::tie(e, inserted) = add_edge(*i, superNode, g);
		if (!inserted) cout << "insert edge failed" << endl;
		weightmap[e] = 0;
	}

	//shortest path;
	std::vector<vertex_descriptor> p(vNum);
	std::vector<double> d(vNum);
	vertex_descriptor s = vertex(superNode, g);
	dijkstra_shortest_paths(g, s, predecessor_map(&p[0]).distance_map(&d[0]));

	d.pop_back();
	double maxDist = *std::max_element(d.begin(), d.end());
	res.resize(d.size());
	for (int i = 0; i < d.size(); i++)
	{
		res[i] = maxDist - d[i];
	}
}
void Mitani_WaterShed::distanceToFeature_ShortestPath(MGraph& pg, std::set<int>& sources, std::vector<double>& res)
{
	//build dual graph;

	int superNode = pg.vNum;
	int vNum = pg.vNum + 1; //will add a super node
	int eNum = pg.eNum + sources.size();//will add edges connecting super node and sources

	// init graph
	graph_t g(vNum);
	property_map<graph_t, edge_weight_t>::type weightmap = get(edge_weight, g);
	for (auto i = pg.es.begin(); i != pg.es.end(); i++)
	{
		int nodeID[] = { i->n1, i->n2 };
		graph_traits < graph_t >::edge_descriptor e;
		bool inserted;
		boost::tie(e, inserted) = add_edge(nodeID[0], nodeID[1], g);
		if (!inserted) cout << "insert edge failed" << endl;
		weightmap[e] = i->w;
	}
	for (auto i = sources.begin(); i != sources.end(); i++)
	{
		graph_traits < graph_t >::edge_descriptor e;
		bool inserted;
		boost::tie(e, inserted) = add_edge(*i, superNode, g);
		if (!inserted) cout << "insert edge failed" << endl;
		weightmap[e] = 0;
	}

	//shortest path;
	std::vector<vertex_descriptor> p(vNum);
	std::vector<double> d(vNum);
	vertex_descriptor s = vertex(superNode, g);
	dijkstra_shortest_paths(g, s, predecessor_map(&p[0]).distance_map(&d[0]));

	d.pop_back();
	double maxDist = *std::max_element(d.begin(), d.end());
	res.resize(d.size());
	for (int i = 0; i < d.size(); i++)
	{
		res[i] = maxDist - d[i];
	}
}

int Mitani_WaterShed::grow(const std::vector<std::list<MyMesh::Vertex>::iterator>& vers, const std::vector<double>& distanceToFeature, std::vector<bool>& isExtrema, std::vector<unsigned>& vertexLabels)
{
	//init indexed items, and the flood fronts;
	int vNum = distanceToFeature.size();
	std::vector<bool> unIndexed(vNum, true);

	vertexLabels.clear(); vertexLabels.resize(vNum, UINT_MAX);
	int labelId = 0;
	std::priority_queue<HighFunction> floodFront;
	for (int i = 0; i < vNum; i++)
	{
		if (isExtrema[i])
		{
			unIndexed[i] = false;
			vertexLabels[i] = labelId++;
			floodFront.push(HighFunction(i, distanceToFeature[i]));
		}
	}

	const auto vts = vers;
	while (!floodFront.empty())
	{
		HighFunction v = floodFront.top(); floodFront.pop();

		for (int i = 0; i < vts[v.m_id]->vertex_iter().size(); i++)
		{
			int id = vts[v.m_id]->vertex_iter()[i]->id();
			if (unIndexed[id]) //unindexed neighbore;
			{
				HighFunction newV(id, distanceToFeature[id]);
				floodFront.push(newV);

				unIndexed[id] = false;
				vertexLabels[id] = vertexLabels[v.m_id];
			}
		}
	}

	cout << "cluster number:" << labelId << endl;
	return labelId;
}
//////////////////////////////////////////////////////////////////////////
//grow2 -- Implement of Paper: Analysis and Comparison of Algorithms for Morse Decompositions on Triangulated Terrains
//by Maria Vitali, Leila De Floriani, Paola Magillo
//detailed in section 5:watershed algorithms, page 9.
//the output contains not just the cluster labels of vertices, but also the boundary vertices(between basins).
int Mitani_WaterShed::grow2(MGraph& pg, const std::vector<double>& distanceToFeature, const std::vector<std::set<unsigned> >& neighbours, std::vector<unsigned>& vertexLabels)
{
	//init indexed items, and the flood fronts;
	int vNum = distanceToFeature.size();
	std::vector<bool> unIndexed(vNum, true);
	std::priority_queue<HighFunction> floodFront;
	for (int i = 0; i < vNum; i++)
	{
		floodFront.push(HighFunction(i, distanceToFeature[i]));
	}

	int labelId = 0;
	std::vector<int> labels(vNum, -1);
	while (!floodFront.empty())
	{
		HighFunction v = floodFront.top(); floodFront.pop();
		std::set<int> ls;
		for (auto n = neighbours[v.m_id].begin(); n != neighbours[v.m_id].end(); n++)
		{
			if (!unIndexed[*n] && labels[*n] != -2) //neighbours assigned as basin;
			{
				ls.insert(labels[*n]);
			}
		}

		if (ls.empty())
		{
			labels[v.m_id] = labelId; labelId++;//new cluster begins...
		}
		else if (ls.size() == 1)
		{
			labels[v.m_id] = *ls.begin();//basin node
		}
		else
		{
			labels[v.m_id] = -2;//watershed node
		}

		unIndexed[v.m_id] = false;
	}

	vertexLabels.resize(vNum, 0);
	for (int i = 0; i < vNum; i++)
	{
		if (labels[i] == -2)
		{
			vertexLabels[i] = UINT_MAX; //watershed node
		}
	}

	return labelId;
}
void Mitani_WaterShed::thresholdClusters(MyMesh* mesh, std::vector<unsigned>& faceLabels, unsigned& segNum, std::vector<bool>& isFeature, double thres)
{
	// In our case, we use area as measurement of cluster size, for merging. Other criteria, like perimeter, would only require few changes of the code.
	double averageArea = 0;
	std::vector<double> faceAreas(mesh->getFaces().size());
	for (auto i = mesh->getFaces().begin(); i != mesh->getFaces().end(); i++)
	{
		faceAreas[i->id()] = i->triangleCost(2);
		averageArea += faceAreas[i->id()];
	}

	thres = averageArea * thres*0.01; //thres percent (thres%) of total area;

	//initialize area of all clusters;
	std::vector<double> scores(segNum, 0.0);
	for (auto i = mesh->getFaces().begin(); i != mesh->getFaces().end(); i++)
	{
		scores[faceLabels[i->id()]] += faceAreas[i->id()];
	}

	//shared boundary length between clusters;
	std::vector<double> sharesRow(segNum, 0.0);
	std::vector<std::vector<double> > sharesMatrix(segNum, sharesRow);
	for (auto i = mesh->getEdges().begin(); i != mesh->getEdges().end(); i++)
	{
		if (i->manifold())
		{
			int l1 = faceLabels[i->face_iter(0)->id()];
			int l2 = faceLabels[i->face_iter(1)->id()];

			if (l1 != l2 && !isFeature[i->face_iter(0)->id()] && !isFeature[i->face_iter(1)->id()])
			{
				sharesMatrix[l1][l2] = sharesMatrix[l2][l1] += i->length(); //in dual graph case, use i->dualLength;
			}
		}
	}

	//build adjacency and priority queue;
	std::multiset<ClusterArea> clusterQueue;
	for (int i = 0; i < scores.size(); i++)
	{
		clusterQueue.insert(ClusterArea(i, scores[i]));
	}
	if (clusterQueue.size() != scores.size())
	{
		cout << "numerical issue" << endl;
	}
	//adjcents between clusters;
	std::list<std::multiset<DecreaseOrderExt> > clusterAdjcencyList(segNum);
	std::vector<std::list<std::multiset<DecreaseOrderExt> >::iterator> clusterAdjcency;
	for (auto i = clusterAdjcencyList.begin(); i != clusterAdjcencyList.end(); i++)
	{
		clusterAdjcency.push_back(i);
	}

	for (auto i = mesh->getEdges().begin(); i != mesh->getEdges().end(); i++)
	{
		if (i->manifold())
		{
			int l1 = faceLabels[i->face_iter(0)->id()];
			int l2 = faceLabels[i->face_iter(1)->id()];

			if (l1 != l2)
			{
				if (clusterAdjcency[l1]->find(DecreaseOrderExt(l2, scores[l2], sharesMatrix[l1][l2])) == clusterAdjcency[l1]->end())
					clusterAdjcency[l1]->insert(DecreaseOrderExt(l2, scores[l2], sharesMatrix[l1][l2]));

				if (clusterAdjcency[l2]->find(DecreaseOrderExt(l1, scores[l1], sharesMatrix[l1][l2])) == clusterAdjcency[l2]->end())
					clusterAdjcency[l2]->insert(DecreaseOrderExt(l1, scores[l1], sharesMatrix[l1][l2]));
			}
		}
	}

	//clusters
	std::list<std::set<int>> clusters(segNum);
	std::vector<std::list<std::set<int>>::iterator> iclusters;
	for (auto i = clusters.begin(); i != clusters.end(); i++)
	{
		iclusters.push_back(i);
	}
	for (int i = 0; i < faceLabels.size(); i++)
	{
		iclusters[faceLabels[i]]->insert(i);
	}

// 	cout << "thres:" << thres << endl;
	while (!clusterQueue.empty())
	{
		ClusterArea ci = *clusterQueue.begin(); clusterQueue.erase(clusterQueue.begin());
		if (ci.m_val > thres)
		{
			break;
		}
		if (clusterAdjcency[ci.m_id]->empty())
		{
			continue;
		}

		DecreaseOrderExt cj_ = *clusterAdjcency[ci.m_id]->begin();
		ClusterArea cj(cj_.m_id, cj_.m_val);

		clusterAdjcency[ci.m_id]->erase(clusterAdjcency[ci.m_id]->begin()); //remove cj from ci's

		auto tp = clusterAdjcency[cj.m_id]->begin();
		for (; tp != clusterAdjcency[cj.m_id]->end(); tp++)
		{
			if (tp->m_id == ci.m_id )
				break;
		}
		if (tp == clusterAdjcency[cj.m_id]->end())
		{
			cout << "invalid merge" << endl;
		}
		else
		{
			clusterAdjcency[cj.m_id]->erase(tp);
		}

		auto i = std::find(clusterQueue.begin(), clusterQueue.end(), cj);
		if (i == clusterQueue.end())
		{
			cout << "invalid reference" << endl;
		}
		else
		{
			clusterQueue.erase(i);
		}

		//0. merge ci to cj.
		//1. cj is still in clusterQueue, need to update it, the new area is not just sum of ci and cj.
		//2. clear ci's clusterAdjacency, and add adj to cj, if not existed in cj. add cj to ci's adjacency, if not existed.

		//0. merge ci to cj.
		for (auto i = iclusters[ci.m_id]->begin(); i != iclusters[ci.m_id]->end(); i++)
		{
			faceLabels[*i] = cj.m_id;
		}
		iclusters[cj.m_id]->insert(iclusters[ci.m_id]->begin(), iclusters[ci.m_id]->end());
		iclusters[ci.m_id]->clear();

		//1. cj is still in clusterQueue, need to update it, the new area is  just sum of ci and cj.
		//2. clear ci's clusterAdjacency, and add it's adjacency to cj, if not existed in cj, and cj to it's adjacency, if not existed in...
		// ci's = {ci_a, ci_...}
		double newScore = ci.m_val + cj.m_val;
		clusterQueue.insert(ClusterArea(cj.m_id,newScore));

		for (auto i = clusterAdjcency[ci.m_id]->begin(); i != clusterAdjcency[ci.m_id]->end(); i++)
		{
			int ci_a = i->m_id; // for each cluster

			//if ci_a not in cj, then add to cj, if exist, then update their shared boundary, m_val_ext;
			//if cj not in ci_a, then add to ci_a, if exist, then update their shared boundary, m_val_ext;
			auto ip = clusterAdjcency[ci_a]->begin();
			for (; ip != clusterAdjcency[ci_a]->end(); ip++)
			{
				if (ip->m_id == cj.m_id && ip->m_val == cj.m_val)
					break;
			}
			if (ip == clusterAdjcency[ci_a]->end())
			{
				clusterAdjcency[ci_a]->insert(DecreaseOrderExt(cj.m_id, newScore, i->m_val_ext));
			}
			else
			{
				double temp = ip->m_val_ext;
				clusterAdjcency[ci_a]->erase(ip);
				clusterAdjcency[ci_a]->insert(DecreaseOrderExt(cj.m_id, newScore, i->m_val_ext + temp));
			}

			ip = clusterAdjcency[cj.m_id]->begin();
			for (; ip != clusterAdjcency[cj.m_id]->end(); ip++)
			{
				if (ip->m_id == i->m_id && ip->m_val == i->m_val)
					break;
			}
			if (ip == clusterAdjcency[cj.m_id]->end())
			{
				clusterAdjcency[cj.m_id]->insert(DecreaseOrderExt(i->m_id, i->m_val, i->m_val_ext));
			}
			else
			{
				double temp = ip->m_val_ext;
				clusterAdjcency[cj.m_id]->erase(ip);
				clusterAdjcency[cj.m_id]->insert(DecreaseOrderExt(i->m_id, i->m_val, i->m_val_ext + temp));
			}

			//remove ci from ci_a
			ip = clusterAdjcency[ci_a]->begin();
			for (; ip != clusterAdjcency[ci_a]->end(); ip++)
			{
				if (ip->m_id == ci.m_id && ip->m_val == ci.m_val)
					break;
			}
			if (ip == clusterAdjcency[ci_a]->end())
			{
				cout << endl << ci_a << " error " << ci.m_id << " " << ci.m_val << endl;
			}
			else
			{
				clusterAdjcency[ci_a]->erase(ip);
			}
		}

		clusterAdjcency[ci.m_id]->clear();

		//for all cj_a in cj, even though not appeared in ci, need to update cj_a's element cj...
		for (auto i = clusterAdjcency[cj.m_id]->begin(); i != clusterAdjcency[cj.m_id]->end();i++)
		{
			auto ip = clusterAdjcency[i->m_id]->begin();
			for (; ip != clusterAdjcency[i->m_id]->end(); ip++)
			{
				if (ip->m_id == cj.m_id)
					break;
			}
			if (ip == clusterAdjcency[i->m_id]->end())
			{
				cout << "bad reference" << endl;
			}
			else
			{
				clusterAdjcency[i->m_id]->erase(ip);
				clusterAdjcency[i->m_id]->insert(DecreaseOrderExt(cj.m_id, newScore,i->m_val_ext));
			}
		}

	}

	//update label;
	int ind = 0;
	for (auto i = clusters.begin(); i != clusters.end(); i++)
	{
		if (i->empty())
			continue;

		for (auto f = i->begin(); f != i->end(); f++)
		{
			faceLabels[*f] = ind;
		}

		ind++;
	}
	segNum = ind;
}

void MeshSegment::convertFaceToVertexLabelling(std::vector<unsigned>& vertexLabel,std::vector<unsigned>& faceLabel)
{
	unsigned vNum = myMesh->getVertices().size();
	std::vector<std::vector<unsigned> > vertexMultiLabels(vNum);
	for (auto i = myMesh->getFaces().begin(); i != myMesh->getFaces().end(); i++)
	{
		for (unsigned j = 0; j < 3; j++)
		{
			vertexMultiLabels[i->vertex_iter(j)->id()].push_back(faceLabel[i->id()]);
		}
	}

	vertexLabel.clear(); vertexLabel.resize(vNum);
	for (int i = 0; i < vNum; i++)
	{
		std::map<unsigned, unsigned> st;
		std::map<unsigned, unsigned>::iterator it;
		for (int j = 0; j < vertexMultiLabels[i].size(); j++)
		{
			it = st.find(vertexMultiLabels[i][j]);
			if (it == st.end())
			{
				st[vertexMultiLabels[i][j]] = 1;
			}
			else
			{
				st[vertexMultiLabels[i][j]] = it->second + 1;
			}
		}

		std::pair<unsigned, unsigned> majorPair(vertexMultiLabels[i][0],1);
		for (it = st.begin(); it != st.end(); it++)
		{
			if (majorPair.second < it->second)
			{
				majorPair.first = it->first;
				majorPair.second = it->second;
			}
		}
		vertexLabel[i] = majorPair.first;
	}

	std::vector<double> weights;
	getWeights(edgeWeightParameter,weights);

	double cutCost = 0;
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		if (vertexLabel[e->vertex_iter(0)->id()] != vertexLabel[e->vertex_iter(1)->id()])
		{
			cutCost += weights[e->id()];
			graphFeature.ef[e->id()].isCut = true;
		}
		else
		{
			graphFeature.ef[e->id()].isCut = false;
		}
	}
	cout << "Cost:" << cutCost << endl;

	unsigned eNum = myMesh->getEdges().size();
	vertexLabel.clear(); vertexLabel.resize(vNum);
	segNumber = 0;
	std::vector<bool> visitedVer(vNum, false);
	std::vector<bool> visitedEdge(eNum, false);
	for (int i = 0; i < vNum; i++)
	{
		if (visitedVer[i] == true) continue;

		std::vector<int> frontVer(1, i);
		//propagates vertex with same label, and group them into a subgraph
		while (!frontVer.empty())
		{
			int fv = frontVer.back(); frontVer.pop_back();
			vertexLabel[fv] = segNumber;

			auto v = mg.vs[fv].vadjs.begin();
			auto e = mg.vs[fv].eadjs.begin();
			for (; v != mg.vs[fv].vadjs.end(); v++, e++)
			{
				if (graphFeature.ef[*e].isCut || visitedEdge[*e] == true)
					continue;

				visitedEdge[*e] = true;

				if (visitedVer[*v] == true)
					continue;

				visitedVer[*v] = true;

				frontVer.push_back(*v);
			}
		}
		segNumber++;
	}

	cout << endl << "cluster number :" << segNumber << endl;
}
void MeshSegment::convertVertexToFaceLabelling(std::vector<unsigned>& vertexLabel,std::vector<unsigned>& faceLabel)
{
	unsigned fNum = myMesh->getFaces().size();
	std::vector<std::vector<unsigned> > faceMultiLabels(fNum);
	for (auto i = myMesh->getFaces().begin(); i != myMesh->getFaces().end(); i++)
	{
		for (unsigned j = 0; j < 3; j++)
		{
			faceMultiLabels[i->id()].push_back(vertexLabel[i->vertex_iter(j)->id()]);
		}
	}

	faceLabel.clear();	faceLabel.resize(fNum);
	for (int i = 0; i < fNum; i++)
	{
		std::map<unsigned, unsigned> st;
		std::map<unsigned, unsigned>::iterator it;
		for (int j = 0; j < faceMultiLabels[i].size(); j++)
		{
			it = st.find(faceMultiLabels[i][j]);
			if (it == st.end())
			{
				st[faceMultiLabels[i][j]] = 1;
			}
			else
			{
				st[faceMultiLabels[i][j]] = it->second + 1;
			}
		}

		std::pair<unsigned, unsigned> majorPair(faceMultiLabels[i][0], 1);
		for (it = st.begin(); it != st.end(); it++)
		{
			if (majorPair.second < it->second)
			{
				majorPair.first = it->first;
				majorPair.second = it->second;
			}
		}

		faceLabel[i] = majorPair.first;
	}
}

///////////////////OVER SEGMENTATION///////////////////////////////
void MeshSegment::Mitani_Watershed()
{
	clock_t tstr = clock();

	//find sources;
	std::set<int> sources;
	for (auto e_it = myMesh->getEdges().begin(); e_it != myMesh->getEdges().end(); e_it++)
	{
		if (graphFeature.ef[e_it->id()].lab != -1)
		{
			sources.insert(e_it->vertex_iter(0)->id());
			sources.insert(e_it->vertex_iter(1)->id());
		}
	}

	ws.distanceToFeature_ShortestPath(myMesh->getVertices(), myMesh->getEdges(), sources, ws.distanceToFeature);

	//local extrema, and growing
	auto& vers = myMesh->getVIter();
	std::vector<bool> isExtrema(vers.size(), true);
	int ringSize = mitani_Watershed_Ringsize;
	for (int i = 0; i < vers.size(); i++)
	{
		int v1 = i;
		if (isExtrema[v1])
		{
			int times = ringSize;
			std::set<int> fronts; fronts.insert(v1);
			std::set<int> visited = fronts;
			std::set<int> nextFronts = fronts;
			while (times > 0)
			{
				for (auto j = fronts.begin(); j != fronts.end(); j++)
				{
					auto vj = vers[*j];
					for (int k = 0; k < vj->vertex_iter().size(); k++)
					{
						int v2 = vj->vertex_iter()[k]->id();
						if (visited.find(v2)!=visited.end())continue;
						nextFronts.insert(v2);
						visited.insert(v2);
					}
				}
				fronts = nextFronts;
				times--;
			}

			visited.erase(v1);
			for (auto j = visited.begin(); j != visited.end(); j++)
			{
				int v2 = *j;
				ws.distanceToFeature[v1] < ws.distanceToFeature[v2] ? isExtrema[v2] = false : isExtrema[v1] = false;
			}
		}
	}

	auto& multiCutLabels = vertexLabel;
	auto& multiCutNum = segNumber;
	multiCutNum = ws.grow(myMesh->getVIter(), ws.distanceToFeature, isExtrema, multiCutLabels);

	clock_t totalTime = clock() - tstr;
	cout << "Watershed Segmentation Use " << totalTime / 1000 << "sec" << totalTime % 1000 << "mm" << endl;

	std::vector<double> weights;
	getWeights(edgeWeightParameter, weights);

	vertexLabelBeforeMerge = vertexLabel;

	double cutCost = 0;
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		if (multiCutLabels[e->vertex_iter(0)->id()] != multiCutLabels[e->vertex_iter(1)->id()])
		{
			cutCost += weights[e->id()];
			graphFeature.ef[e->id()].isCut = true;
		}
		else
		{
			graphFeature.ef[e->id()].isCut = false;
		}
	}
	cout << "Cost:" << cutCost << endl;
}
void MeshSegment::Mitani_Watershed_Dual()
{
	clock_t tstr = clock();

	//build dual graph;
	std::list<MyMesh::Vertex> versArray(myMesh->getFaces().size());
	std::vector<std::list<MyMesh::Vertex>::iterator> vers(myMesh->getFaces().size());
	int ind = 0;
	for (auto v = versArray.begin(); v != versArray.end(); v++, ind++)
	{
		v->id() = ind;
		vers[ind] = v;
	}

	for (auto f = myMesh->getFaces().begin(); f != myMesh->getFaces().end(); f++)
	{
		vers[f->id()]->coordinate() = f->center_point();
	}

	std::vector<double> edgeCosts(myMesh->getEdges().size());
	std::vector<double> edgeLengths(myMesh->getEdges().size());
	{
		std::vector<Tensor>& tAnis = faceAnis;

		for (auto e_it = myMesh->getEdges().begin(); e_it != myMesh->getEdges().end(); e_it++)
		{
			if (e_it->manifold())
			{
				MyMesh::FaceIter f[] = { e_it->face_iter(0), e_it->face_iter(1) };
				unsigned fid[] = { f[0]->id(), f[1]->id() };
				Vec3 edgeVec = graphFeature.cf[f[0]->id()].rep - graphFeature.cf[f[1]->id()].rep;
				double edgeCost = 0;
				for (unsigned k = 0; k < 2; k++)
				{
					Vec2 edgeVec2D = Vec2(edgeVec.dot(tAnis[fid[k]].dir1), edgeVec.dot(tAnis[fid[k]].dir2));// two direction were swapped, since we computed dual edge's cost; and in faceAnis, mag1>mag2;
					edgeCost += sqrt(pow(edgeVec2D.x, 2)*tAnis[fid[k]].mag1 + pow(edgeVec2D.y, 2)*tAnis[fid[k]].mag2);
				}
				edgeCosts[e_it->id()] = edgeCost*0.5;
				edgeLengths[e_it->id()] = edgeVec.length();
			}
			else
			{
				MyMesh::FaceIter f[] = { e_it->face_iter(0) };
				unsigned fid[] = { f[0]->id(), f[1]->id() };
				Vec3 edgeVec = graphFeature.cf[f[0]->id()].rep - graphFeature.ef[e_it->id()].rep;
				double edgeCost = 0;
				for (unsigned k = 0; k < 1; k++)
				{
					Vec2 edgeVec2D = Vec2(edgeVec.dot(tAnis[fid[k]].dir1), edgeVec.dot(tAnis[fid[k]].dir2));// two direction were swapped, since we computed dual edge's cost; and in faceAnis, mag1>mag2;
					edgeCost += sqrt(pow(edgeVec2D.x, 2)*tAnis[fid[k]].mag1 + pow(edgeVec2D.y, 2)*tAnis[fid[k]].mag2);
				}
				edgeCosts[e_it->id()] = edgeCost + 1e-6; //make sure the boundary edge has weight larger than 0;
				edgeLengths[e_it->id()] = edgeVec.length();
			}
		}
	}

	std::list<MyMesh::Edge> edgesArray;
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		if (!e->manifold()) continue;

		int f1 = e->face_iter(0)->id();
		int f2 = e->face_iter(1)->id();

		vers[f1]->vertex_iter().push_back(vers[f2]);
		vers[f2]->vertex_iter().push_back(vers[f1]);

		MyMesh::Edge de;
		{//compute anis distance
			if (isAnisGeodesics_Watershed)
			{
				de.length() = edgeCosts[e->id()];
			}
			else
			{
				de.length() = edgeLengths[e->id()];
			}
		}
		de.vertex_iter(0) = vers[f1];
		de.vertex_iter(1) = vers[f2];
		edgesArray.push_back(de);
	}
	std::vector<std::list<MyMesh::Edge>::iterator> edges(edgesArray.size());
	ind = 0;
	for (auto e = edgesArray.begin(); e != edgesArray.end(); e++, ind++)
	{
		e->id() = ind;
		edges[ind] = e;
	}

	//find sources;
	std::set<int> sources;
	for (unsigned i = 0; i<crestEdgesVisible.size(); i++)
	{
		if (!crestEdgesVisible[i])
			continue;

		unsigned faceID = crestEdges[i][2];
		if (faceID>myMesh->getFaces().size())
		{
			continue; // crest line data is not alway clean
		}
		sources.insert(faceID);
	}
	for (auto f = userSketches.begin(); f != userSketches.end(); f++)
	{
		sources.insert(f->fid);
	}

	ws.distanceToFeature_ShortestPath(versArray, edgesArray, sources, ws.distanceToFeature);

	//local extrema, and growing
	std::vector<std::set<unsigned> > neighbours(myMesh->getFaces().size());
	for (auto f = myMesh->getFaces().begin(); f != myMesh->getFaces().end(); f++)
	{
		auto& fs = neighbours[f->id()];
		for (unsigned j = 0; j < 3; j++)
		{
			for (auto e = 0; e < f->vertex_iter(j)->edge_iter().size(); e++)
			{
				fs.insert(f->vertex_iter(j)->edge_iter()[e]->face_iter(0)->id());
				if (f->vertex_iter(j)->edge_iter()[e]->manifold())
					fs.insert(f->vertex_iter(j)->edge_iter()[e]->face_iter(1)->id());
			}
		}
	}

// 	std::vector<bool> isExtrema(neighbours.size(), true);
// 	for (unsigned i = 0; i < neighbours.size(); i++)
// 	{
// 		for (auto j = neighbours[i].begin(); j != neighbours[i].end(); j++)
// 		{
// 			if(ws.distanceToFeature[i] > ws.distanceToFeature[*j])
// 			{
// 				isExtrema[i] = false;
// 			}
// 		}
// 	}

	std::vector<unsigned> watershedLabels;
	ws.grow2(mg,ws.distanceToFeature, neighbours, watershedLabels);

	clock_t totalTime = clock() - tstr;
	cout << "Watershed Segmentation Use " << totalTime / 1000 << "sec" << totalTime % 1000 << "mm" << endl;

	std::vector<double> weights;
	getWeights(edgeWeightParameter, weights);

	double cutCost = 0;
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		if (!e->manifold()) continue;

		if (watershedLabels[e->face_iter(0)->id()] == UINT_MAX && watershedLabels[e->face_iter(1)->id()] == UINT_MAX)
		{
			cutCost += weights[e->id()];
			graphFeature.ef[e->id()].isCut = true;
		}
		else
		{
			graphFeature.ef[e->id()].isCut = false;
		}
	}
	cout << "Cost:" << cutCost << endl;

	unsigned vNum = myMesh->getVertices().size();
	unsigned eNum = myMesh->getEdges().size();
	vertexLabel.clear(); vertexLabel.resize(vNum);
	segNumber = 0;
	std::vector<bool> visitedVer(vNum, false);
	std::vector<bool> visitedEdge(eNum, false);
	for (int i = 0; i < vNum; i++)
	{
		if (visitedVer[i] == true) continue;

		std::vector<int> frontVer(1, i);
		//propagates vertex with same label, and group them into a subgraph
		while (!frontVer.empty())
		{
			int fv = frontVer.back(); frontVer.pop_back();
			vertexLabel[fv] = segNumber;

			auto v = mg.vs[fv].vadjs.begin();
			auto e = mg.vs[fv].eadjs.begin();
			for (; v != mg.vs[fv].vadjs.end(); v++, e++)
			{
				if (graphFeature.ef[*e].isCut || visitedEdge[*e] == true)
					continue;

				visitedEdge[*e] = true;

				if (visitedVer[*v] == true)
					continue;

				visitedVer[*v] = true;

				frontVer.push_back(*v);
			}
		}
		segNumber++;
	}

	cout << endl << "cluster number:" << segNumber << endl;

	vertexLabelBeforeMerge = vertexLabel;
}
////////////////////MERGING//////////////////////////////////
void MeshSegment::LMP_Merging(const std::vector<unsigned>& vertexLabel, std::vector< std::list< std::list< MGTriple > >::iterator >&pCIter,
	unsigned labelId, std::vector<unsigned>& newLabels)
{
	unsigned numE = 0;
	unsigned numV = labelId;
	std::vector<std::pair<std::pair<unsigned, unsigned>, double> > edges;
	for (int i = 0; i < pCIter.size(); i++){
		for (auto tpc = pCIter[i]->begin(); tpc != pCIter[i]->end(); tpc++)
		{
			if (i != tpc->j)
			{
				numE++;
				edges.push_back(std::pair<std::pair<unsigned, unsigned>, double>(std::pair<unsigned, unsigned>(i, tpc->j), tpc->val));
			}
		}
	}
	cout << "number of edges:" << numE << "  number of vertices:" << numV << endl;

	andres::graph::Graph<> graph;
	graph.insertVertices(numV);
	std::vector<double> weights(numE);
	for (int i = 0; i < numE; i++)
	{
		graph.insertEdge(edges[i].first.first, edges[i].first.second);
		weights[i] = edges[i].second;
	}

	std::vector<char> edge_labels(graph.numberOfEdges(), 1);
	andres::graph::multicut_lifted::greedyAdditiveEdgeContraction(graph, graph, weights, edge_labels);

	std::vector<char> out_labels(graph.numberOfEdges(), 1);
	andres::graph::multicut_lifted::kernighanLin(graph, graph, weights, edge_labels, out_labels);

	std::vector<unsigned> res(numV);
	std::vector<bool> visitedVer(numV, false);
	std::vector<bool> visitedEdge(numE, false);
	unsigned multiCutNum = 0;
	for (int i = 0; i < numV; i++)
	{
		if (visitedVer[i] == true) continue;

		std::vector<int> frontVer(1, i);
		//propagates vertex with same label, and group them into a subgraph
		while (!frontVer.empty())
		{
			int fv = frontVer.back(); frontVer.pop_back();
			res[fv] = multiCutNum;

			for (auto p = graph.adjacenciesToVertexBegin(fv); p != graph.adjacenciesToVertexEnd(fv); p++)
			{
				auto eid = p->edge();
				if (out_labels[eid] || visitedEdge[eid] == true)
					continue;

				visitedEdge[eid] = true;

				if (visitedVer[p->vertex()] == true)
					continue;

				visitedVer[p->vertex()] = true;

				frontVer.push_back(p->vertex());
			}
		}
		multiCutNum++;
	}
	//////////////////////////////////////////////////////////////////////////
	newLabels.clear(); newLabels.resize(vertexLabel.size());
	for (size_t i = 0; i < vertexLabel.size(); i++)
	{
		newLabels[i] = res[vertexLabel[i]];
	}
}
void MeshSegment::Mitani_Watershed_Dual_Partial(std::vector<unsigned>& fs)
{
	if (fs.empty())return;

	std::set<int> patchLabels;
	for (int i = 0; i < fs.size(); i++)
	{
		auto&f = myMesh->getFIter()[fs[i]];
		for (int j = 0; j < 3; j++)
		{
			patchLabels.insert(vertexLabel[f->vertex_iter(j)->id()]);
		}
	}

	std::vector<bool> faceInPatches(myMesh->getFaces().size(), true);
	subgraphVers.clear(); subgraphVers.resize(myMesh->getVertices().size(), true);
	unsigned numV = faceInPatches.size();
	for (auto f = myMesh->getFaces().begin(); f != myMesh->getFaces().end(); f++)
	{
		for (int j = 0; j < 3; j++)
		{
			if (std::find(patchLabels.begin(), patchLabels.end(), vertexLabel[f->vertex_iter(j)->id()]) == patchLabels.end())
			{
				faceInPatches[f->id()] = false;
				subgraphVers[f->vertex_iter(j)->id()] = false;
			}
		}
		if (faceInPatches[f->id()] == false) numV--;
	}

	MGraph pg;//partial graph;
	pg.vNum = numV;
	pg.vs.clear(); pg.vs.resize(pg.vNum);

	std::map<unsigned, unsigned> localIndToGlobalInd;
	std::map<unsigned, unsigned> globalIndToLocalInd;
	unsigned ind = 0;
	for (auto f = myMesh->getFaces().begin(); f != myMesh->getFaces().end(); f++)
	{
		if (faceInPatches[f->id()])
		{
			pg.vs[ind].id = ind;
			pg.vs[ind].coord = f->center_point();
			localIndToGlobalInd[ind] = f->id();
			globalIndToLocalInd[f->id()] = ind;
			ind++;
		}
	}

	pg.es.clear();
	ind = 0;
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		if (!e->manifold()) continue;

		int f1 = e->face_iter(0)->id();
		int f2 = e->face_iter(1)->id();
		if (!faceInPatches[f1] || !faceInPatches[f2]) continue;

		f1 = globalIndToLocalInd[f1];
		f2 = globalIndToLocalInd[f2];

		pg.vs[f1].vadjs.push_back(f2);
		pg.vs[f2].vadjs.push_back(f1);

		MGraph::Edge de;
		de.w = e->cost();
		de.n1 = f1;
		de.n2 = f2;
		pg.es.push_back(de);

		pg.vs[f1].eadjs.push_back(ind);
		pg.vs[f2].eadjs.push_back(ind);
		ind++;
	}
	pg.eNum = pg.es.size();

	//find sources;
	std::set<int> sources;
	{
		for (unsigned i = 0; i < crestEdgesVisible.size(); i++)
		{
			if (!crestEdgesVisible[i])
				continue;

			unsigned faceID = crestEdges[i][2];
			if (faceID > myMesh->getFaces().size())
			{
				continue; // crest line data is not alway clean
			}
			if (!faceInPatches[faceID]) continue;

			sources.insert(globalIndToLocalInd[faceID]);
		}
		if (!graphCutLocally)
		{
			for (unsigned i = 0; i < userSketches.size(); i++)
			{
				if (!faceInPatches[userSketches[i].fid])continue;

				sources.insert(globalIndToLocalInd[userSketches[i].fid]);
			}
			for (unsigned i = 0; i < fs.size(); i++)
			{
				sources.insert(globalIndToLocalInd[fs[i]]);
			}
		}
	}

	std::vector<double> distanceToFeature;
	ws.distanceToFeature_ShortestPath(pg, sources, distanceToFeature);

	//localhood
	std::vector<std::set<unsigned> > neighbours(pg.vNum);
	for (auto f = myMesh->getFaces().begin(); f != myMesh->getFaces().end(); f++)
	{
		if (faceInPatches[f->id()] == false)continue;

		auto& fs = neighbours[globalIndToLocalInd[f->id()]];
		for (unsigned j = 0; j < 3; j++)
		{
			for (auto e = 0; e < f->vertex_iter(j)->edge_iter().size(); e++)
			{
				unsigned tid = f->vertex_iter(j)->edge_iter()[e]->face_iter(0)->id();
				if (faceInPatches[tid])
					fs.insert(globalIndToLocalInd[tid]);
				if (f->vertex_iter(j)->edge_iter()[e]->manifold())
				{
					tid = f->vertex_iter(j)->edge_iter()[e]->face_iter(1)->id();
					if (faceInPatches[tid])
						fs.insert(globalIndToLocalInd[tid]);
				}
			}
		}
	}

	std::vector<unsigned> watershedLabels;
	ws.grow2(pg, distanceToFeature, neighbours, watershedLabels);

	double cutCost = 0;
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		if (!e->manifold()) continue;

		if (vertexLabel[e->vertex_iter(0)->id()] != vertexLabel[e->vertex_iter(1)->id()])
			graphFeature.ef[e->id()].isCut = true;
		else
			graphFeature.ef[e->id()].isCut = false;

		int f1 = e->face_iter(0)->id();
		int f2 = e->face_iter(1)->id();

		if (!faceInPatches[f1] && !faceInPatches[f2])
			continue;
		else if (faceInPatches[f1] && !faceInPatches[f2] && watershedLabels[globalIndToLocalInd[f1]] == UINT_MAX)
		{
			graphFeature.ef[e->id()].isCut = true;
			continue;
		}
		else if (!faceInPatches[f1] && faceInPatches[f2] && watershedLabels[globalIndToLocalInd[f2]] == UINT_MAX)
		{
			graphFeature.ef[e->id()].isCut = true;
			continue;
		}

		if (watershedLabels[globalIndToLocalInd[f1]] == UINT_MAX && watershedLabels[globalIndToLocalInd[f2]] == UINT_MAX)
		{
			graphFeature.ef[e->id()].isCut = true;
		}
		else
		{
			graphFeature.ef[e->id()].isCut = false;
		}
	}

	unsigned vNum = myMesh->getVertices().size();
	unsigned eNum = myMesh->getEdges().size();
	vertexLabel.clear(); vertexLabel.resize(vNum);
	segNumber = 0;
	std::vector<bool> visitedVer(vNum, false);
	std::vector<bool> visitedEdge(eNum, false);
	for (int i = 0; i < vNum; i++)
	{
		if (visitedVer[i] == true) continue;

		std::vector<int> frontVer(1, i);
		//propagates vertex with same label, and group them into a subgraph
		while (!frontVer.empty())
		{
			int fv = frontVer.back(); frontVer.pop_back();
			vertexLabel[fv] = segNumber;

			auto v = mg.vs[fv].vadjs.begin();
			auto e = mg.vs[fv].eadjs.begin();
			for (; v != mg.vs[fv].vadjs.end(); v++, e++)
			{
				if (graphFeature.ef[*e].isCut || visitedEdge[*e] == true)
					continue;

				visitedEdge[*e] = true;

				if (visitedVer[*v] == true)
					continue;

				visitedVer[*v] = true;

				frontVer.push_back(*v);
			}
		}
		segNumber++;
	}

	cout << "cluster number:" << segNumber << endl;

	vertexLabelBeforeMerge = vertexLabel;

	//update initial cut graph;
	if (false)
	{
		std::vector<bool> isCut(myMesh->getEdges().size());
		for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
		{
			if (!e->manifold()) continue;

			if (vertexLabelInit[e->vertex_iter(0)->id()] != vertexLabelInit[e->vertex_iter(1)->id()])
				isCut[e->id()] = true;
			else
				isCut[e->id()] = false;

			int f1 = e->face_iter(0)->id();
			int f2 = e->face_iter(1)->id();

			if (!faceInPatches[f1] && !faceInPatches[f2])
				continue;
			else if (faceInPatches[f1] && !faceInPatches[f2] && watershedLabels[globalIndToLocalInd[f1]] == UINT_MAX)
			{
				isCut[e->id()] = true;
				continue;
			}
			else if (!faceInPatches[f1] && faceInPatches[f2] && watershedLabels[globalIndToLocalInd[f2]] == UINT_MAX)
			{
				isCut[e->id()] = true;
				continue;
			}

			if (watershedLabels[globalIndToLocalInd[f1]] == UINT_MAX && watershedLabels[globalIndToLocalInd[f2]] == UINT_MAX)
			{
				isCut[e->id()] = true;
			}
			else
			{
				isCut[e->id()] = false;
			}
		}
		vertexLabelInit.clear(); vertexLabelInit.resize(vNum);
		unsigned initSegNumber = 0;
		visitedVer.clear(); visitedVer.resize(vNum, false);
		visitedEdge.clear(); visitedEdge.resize(eNum, false);
		for (int i = 0; i < vNum; i++)
		{
			if (visitedVer[i] == true) continue;

			std::vector<int> frontVer(1, i);
			//propagates vertex with same label, and group them into a subgraph
			while (!frontVer.empty())
			{
				int fv = frontVer.back(); frontVer.pop_back();
				vertexLabelInit[fv] = initSegNumber;

				auto v = mg.vs[fv].vadjs.begin();
				auto e = mg.vs[fv].eadjs.begin();
				for (; v != mg.vs[fv].vadjs.end(); v++, e++)
				{
					if (isCut[*e] || visitedEdge[*e] == true)
						continue;

					visitedEdge[*e] = true;

					if (visitedVer[*v] == true)
						continue;

					visitedVer[*v] = true;

					frontVer.push_back(*v);
				}
			}
			initSegNumber++;
		}
	}

	//for debugging
	ws.distanceToFeature.clear();
	for (int i = 0; i < myMesh->getFaces().size(); i++)
	{
		if (globalIndToLocalInd.find(i) == globalIndToLocalInd.end())
			ws.distanceToFeature.push_back(0);
		else
			ws.distanceToFeature.push_back(distanceToFeature[globalIndToLocalInd[i]]);
	}
}
void MeshSegment::LMP_Partitioning()
{
	clock_t tstr = clock();

	unsigned vNum = myMesh->getVertices().size();
	unsigned eNum = myMesh->getEdges().size();

	andres::graph::Graph<> graph;
	graph.insertVertices(vNum);
	for (auto e = myMesh->getEdges().begin(); e != myMesh->getEdges().end(); e++)
	{
		graph.insertEdge(e->vertex_iter(0)->id(), e->vertex_iter(1)->id());
	}
	std::vector<double> weights;
	getWeights(edgeWeightParameter, weights);

	std::vector<char> edge_labels(graph.numberOfEdges(), 1);
	//andres::graph::multicut_lifted::greedyAdditiveEdgeContraction(graph, graph, weights, edge_labels);

	std::vector<char> out_labels(graph.numberOfEdges(), 1);
	//andres::graph::multicut_lifted::kernighanLin(graph, graph, weights, edge_labels, out_labels);

	auto& multiCutLabels = vertexLabel;
	auto& multiCutNum = segNumber;
	multiCutLabels.clear(); multiCutLabels.resize(myMesh->getVertices().size());
	multiCutNum = 0;
	std::vector<bool> visitedVer(vNum, false);
	std::vector<bool> visitedEdge(eNum, false);
	for (int i = 0; i < vNum; i++)
	{
		if (visitedVer[i] == true) continue;

		std::vector<int> frontVer(1, i);
		//propagates vertex with same label, and group them into a subgraph
		while (!frontVer.empty())
		{
			int fv = frontVer.back(); frontVer.pop_back();
			multiCutLabels[fv] = multiCutNum;

			auto v = mg.vs[fv].vadjs.begin();
			auto e = mg.vs[fv].eadjs.begin();
			for (; v != mg.vs[fv].vadjs.end(); v++, e++)
			{
				if (out_labels[*e] || visitedEdge[*e] == true)
					continue;

				visitedEdge[*e] = true;

				if (visitedVer[*v] == true)
					continue;

				visitedVer[*v] = true;

				frontVer.push_back(*v);
			}
		}
		multiCutNum++;
	}

	clock_t tinit = clock() - tstr;
	cout << endl << "LMP takes:" << tinit / 1000 << "sec" << tinit % 1000 << "mm to generate " << multiCutNum << " patches." << endl;
	//////////////////////////////////////////////////////////////////////////

	vertexLabelBeforeMerge = vertexLabel;

	double cutCost = 0;
	for (int i = 0; i < eNum; i++)
	{
		if (out_labels[i])
		{
			cutCost += weights[i];
			graphFeature.ef[i].isCut = true;
		}
		else
		{
			graphFeature.ef[i].isCut = false;
		}
	}
	cout << "Cost:" << cutCost << endl;
}


// Copyright 2020 the Autoware Foundation
//
// 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.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef NDT__UTILS_HPP_
#define NDT__UTILS_HPP_

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <Eigen/Eigenvalues>
#include <limits>

namespace autoware
{
namespace localization
{
namespace ndt
{

/// This function will check if the covariance is valid based on its eigenvalues. If the covariance
/// is valid, eigen values smaller than a fraction of the biggest eigen value will be capped to the
/// threshold. Covariance then will be reconstructed from the modified set of eigen values and
/// vectors. This should result in increased numerical stability as stated in [Magnusson 2009].
/// If the covariance is invalid, it will not be updated and false will be returned.
/// \tparam Derived Deduced Eigen Matrix type.
/// \param covariance [in, out] Covariance matrix to get stabilized.
/// \param scaling_factor [in] The ratio between the max. eigen value and the minimum
/// allowed eigenvalue. Default value is 0.01 as suggested in [Magnusson 2009], page 60.
/// \return True if the covariance matrix is valid.
template<typename Derived>
bool try_stabilize_covariance(
  Eigen::MatrixBase<Derived> & covariance,
  typename Derived::PlainMatrix::Scalar scaling_factor = 0.01)
{
  using CovMatrixT = typename Derived::PlainMatrix;
  using ScalarT = typename CovMatrixT::Scalar;
  using IndexT = typename CovMatrixT::Index;
  constexpr auto TOL = std::numeric_limits<ScalarT>::epsilon();
  Eigen::SelfAdjointEigenSolver<CovMatrixT> solver;
  solver.compute(covariance);

  CovMatrixT evecs = solver.eigenvectors();
  typename decltype(solver)::RealVectorType evals = solver.eigenvalues();
  // Cap the minimum eigen values to scale times the largest eigen value.
  const ScalarT max_e_val = *std::max_element(evals.data(), evals.data() + evals.size());
  const ScalarT min_e_val = max_e_val * scaling_factor;
  if (min_e_val < TOL) {
    return false;
  }
  auto stabilized = false;
  for (auto i = IndexT{0}; i < evals.size(); ++i) {
    ScalarT & e_val = evals(i);
    if (e_val < TOL) {
      // Covariance is not full rank.
      return false;
    }
    if (e_val < min_e_val) {
      e_val = min_e_val;
      stabilized = true;
    }
  }
  if (stabilized) {
    covariance = evecs * evals.asDiagonal() * evecs.inverse();
  }
  return true;
}

template<typename T>
using EigenPose = Eigen::Matrix<T, 6U, 1U>;
template<typename T>
using EigenTransform = Eigen::Transform<T, 3, Eigen::Affine, Eigen::ColMajor>;
using RosTransform = geometry_msgs::msg::Transform;
using RosPose = geometry_msgs::msg::Pose;
namespace transform_adapters
{
/// Template function to convert a 6D pose to a transformation matrix.
/// This function should be specialized and implemented for the supported types.
/// \tparam PoseT Pose type.
/// \tparam TransformT Transform type.
/// \param[in] pose pose to convert
/// \param[out] transform resulting transform
template<typename PoseT, typename TransformT>
void pose_to_transform(const PoseT & pose, TransformT & transform);

/// Template function to convert a 6D pose to a transformation matrix.
/// This function should be specialized and implemented for the supported types.
/// \tparam PoseT Pose type.
/// \tparam TransformT Transform type.
/// \param[in] transform resulting transform
/// \param[out] pose pose to convert
template<typename PoseT, typename TransformT>
void transform_to_pose(const TransformT & transform, PoseT & pose);

template<typename T>
void pose_to_transform(
  const EigenPose<T> & pose,
  EigenTransform<T> & transform)
{
  static_assert(std::is_floating_point<T>::value, "Eigen transform should use floating points");
  transform.setIdentity();
  transform.translation() = pose.head(3);
  transform.rotate(Eigen::AngleAxis<T>(pose(3), Eigen::Vector3d::UnitX()));
  transform.rotate(Eigen::AngleAxis<T>(pose(4), Eigen::Vector3d::UnitY()));
  transform.rotate(Eigen::AngleAxis<T>(pose(5), Eigen::Vector3d::UnitZ()));
}

/// Specialization to convert from the eigen pose to the ros transform type.
/// \tparam T Eigen scalar type
template<typename T>
void pose_to_transform(
  const EigenPose<T> & pose,
  RosTransform & transform)
{
  static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
  Eigen::Quaternion<T> eig_rot{Eigen::Quaternion<T>{}.setIdentity()};
  eig_rot.setIdentity();
  eig_rot =
    Eigen::AngleAxis<T>(pose(3), Eigen::Matrix<T, 3, 1>::UnitX()) *
    Eigen::AngleAxis<T>(pose(4), Eigen::Matrix<T, 3, 1>::UnitY()) *
    Eigen::AngleAxis<T>(pose(5), Eigen::Matrix<T, 3, 1>::UnitZ());

  decltype(RosTransform::translation) trans;
  decltype(RosTransform::rotation) rot;

  trans.set__x(pose(0)).set__y(pose(1)).set__z(pose(2));
  transform.set__translation(trans);

  rot.set__x(eig_rot.x()).
  set__y(eig_rot.y()).
  set__z(eig_rot.z()).
  set__w(eig_rot.w());
  transform.set__rotation(rot);
}

/// Specialization to convert from the eigen pose to the ros pose type.
/// `pose_to_transform` template is used as conversion to `RosPose`
/// is identical to conversion to `RosTransform`
/// \tparam T Eigen scalar type
template<typename T>
void pose_to_transform(
  const EigenPose<T> & pose,
  RosPose & ros_pose)
{
  static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
  Eigen::Quaternion<T> eig_rot{Eigen::Quaternion<T>{}.setIdentity()};
  eig_rot =
    Eigen::AngleAxis<T>(pose(3), Eigen::Matrix<T, 3, 1>::UnitX()) *
    Eigen::AngleAxis<T>(pose(4), Eigen::Matrix<T, 3, 1>::UnitY()) *
    Eigen::AngleAxis<T>(pose(5), Eigen::Matrix<T, 3, 1>::UnitZ());

  decltype(RosPose::position) trans;
  decltype(RosTransform::rotation) rot;

  trans.set__x(pose(0)).set__y(pose(1)).set__z(pose(2));
  ros_pose.set__position(trans);

  rot.set__x(eig_rot.x()).
  set__y(eig_rot.y()).
  set__z(eig_rot.z()).
  set__w(eig_rot.w());
  ros_pose.set__orientation(rot);
}

/// Specialization to convert from the ros pose type to an eigen one.
/// \tparam T Eigen scalar type
template<typename T>
void transform_to_pose(const RosTransform & transform, EigenPose<T> & pose)
{
  static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
  const auto & ros_rot = transform.rotation;
  const auto & ros_trans = transform.translation;
  Eigen::Quaternion<T> eig_rot{ros_rot.w, ros_rot.x, ros_rot.y, ros_rot.z};
  pose(0) = ros_trans.x;
  pose(1) = ros_trans.y;
  pose(2) = ros_trans.z;

  const auto rot = eig_rot.matrix().eulerAngles(0, 1, 2);
  pose(3) = rot(0);
  pose(4) = rot(1);
  pose(5) = rot(2);
}

/// Specialization to convert from the ros pose type to an eigen one.
/// `transform_to_pose` template is used as conversion from `RosPose`
/// is identical to conversion from `RosTransform`
/// \tparam T Eigen scalar type
template<typename T>
void transform_to_pose(const RosPose & ros_pose, EigenPose<T> & pose)
{
  static_assert(std::is_floating_point<T>::value, "Eigen pose should use floating points");
  const auto & ros_rot = ros_pose.orientation;
  const auto & ros_trans = ros_pose.position;
  Eigen::Quaternion<T> eig_rot{ros_rot.w, ros_rot.x, ros_rot.y, ros_rot.z};
  pose(0) = ros_trans.x;
  pose(1) = ros_trans.y;
  pose(2) = ros_trans.z;

  const auto rot = eig_rot.matrix().eulerAngles(0, 1, 2);
  pose(3) = rot(0);
  pose(4) = rot(1);
  pose(5) = rot(2);
}

}  // namespace transform_adapters
}  // namespace ndt
}  // namespace localization
}  // namespace autoware

#endif  // NDT__UTILS_HPP_


//
// Created by michel on 08-01-20.
//

#include <boost/test/unit_test.hpp>
#include <simple-dsp/samplerate.h>
#include <type_traits>

using Integer = uint64_t;
using Real = float;
using IntRate = simpledsp::SampleRateBase<Integer>;
using RealRate = simpledsp::SampleRateBase<Real>;
Integer maxInt = std::numeric_limits<Integer>::max();
Integer maxFloatInt = Integer(1) << std::numeric_limits<Real>::digits;

BOOST_AUTO_TEST_SUITE(SampleRateTests)

BOOST_AUTO_TEST_CASE(sameSampleRateRepresentableInDifferentTypeEqual) {
  Integer rate = 44100;
  IntRate iRate(rate);
  RealRate fRate(rate);
  BOOST_CHECK(iRate == fRate);
  BOOST_CHECK(fRate == iRate);
  BOOST_CHECK(rate == iRate);
  BOOST_CHECK(rate == fRate);
}

BOOST_AUTO_TEST_CASE(sameSampleRatePrecisionLossInDifferentTypeNotEqual) {
  Integer limit = maxFloatInt;
  while (Integer(Real(limit)) == limit) {
    limit++;
  }
  IntRate iRate(limit);
  RealRate fRate(limit);
  BOOST_CHECK(iRate != fRate);
  BOOST_CHECK(fRate != iRate);
  BOOST_CHECK(limit != fRate);
  BOOST_CHECK(limit == iRate);
}

BOOST_AUTO_TEST_CASE(sameSampleRateTooLargeInDifferentTypeNotEqual) {
  Real rate = 1.1 * maxInt;
  IntRate iRate(rate);
  RealRate fRate(rate);
  BOOST_CHECK(iRate != fRate);
  BOOST_CHECK(fRate != iRate);
  BOOST_CHECK(rate == fRate);
  BOOST_CHECK(rate != iRate);
}

BOOST_AUTO_TEST_CASE(sameSampleRateTooSmallInDifferentTypeNotEqual) {
  Real rate = 0.01;
  IntRate iRate(rate);
  RealRate fRate(rate);
  BOOST_CHECK(iRate != fRate);
  BOOST_CHECK(fRate != iRate);
  BOOST_CHECK(rate == fRate);
  BOOST_CHECK(rate != iRate);
}

BOOST_AUTO_TEST_CASE(tooSmallIntegerIsClampedToTwo) {
  Real rate = 0.01;
  IntRate iRate(rate);
  BOOST_CHECK(iRate.rate() == 2);
}

BOOST_AUTO_TEST_CASE(tooSmallRealIsClampedToTwoEpsilon) {
  Real rate = std::numeric_limits<Real>::min() / 5;
  RealRate fRate(rate);
  Real actual = fRate.rate();
  Real expected = std::numeric_limits<Real>::min() * 2;
  BOOST_CHECK(actual == expected);
}

BOOST_AUTO_TEST_CASE(tooLargeIsClampedToMaximum) {
  Real rate = std::numeric_limits<Integer>::max() * 3;
  IntRate fRate(rate);
  Integer actual = fRate.rate();
  Integer expected = std::numeric_limits<Integer>::max();
  BOOST_CHECK(actual == expected);
}

BOOST_AUTO_TEST_CASE(relativeRepresentationTest1) {
  Real fourAndAHalf = 4.5;
  // Truncated to 4, which means 1/9 of error
  BOOST_CHECK(IntRate::representationError(fourAndAHalf) == 0.5 / 4.5);
}

BOOST_AUTO_TEST_CASE(relativeRepresentationTest2) {
  Real fourAndAHalf = 4.4;
  BOOST_CHECK(RealRate::representationError(fourAndAHalf) == 0);
}

BOOST_AUTO_TEST_CASE(relativeRepresentationTest3) {
  Integer limit = maxFloatInt;
  while (Integer(Real(limit)) == limit) {
    limit++;
  }
  Real diff = limit - maxFloatInt;
  double realPresentationError = RealRate::representationError(limit);
  BOOST_CHECK(realPresentationError > 0 &&
              realPresentationError <= diff / maxFloatInt);
}
}


/* --------------------------------------------------------------------------
*
* (C) Copyright …
*
* ---------------------------------------------------------------------------
*/

/*!
 * @file Transform3DEigenTransformConvertersTest.cpp
 * @date 22/01/2018
 * @author Alessandro Bianco
 */

/*!
 * @addtogroup CommonTests
 *
 * Testing conversion from Eigen Transform to Transform3D and viceversa.
 *
 *
 * @{
 */

/* --------------------------------------------------------------------------
 *
 * Includes
 *
 * --------------------------------------------------------------------------
 */
#include <catch.hpp>
#include <Converters/Transform3DToEigenTransformConverter.hpp>
#include <Converters/EigenTransformToTransform3DConverter.hpp>
#include <Types/CPP/Pose.hpp>
#include <Errors/Assert.hpp>
#include <boost/smart_ptr.hpp>
#include <Eigen/Geometry>

using namespace Converters;
using namespace PoseWrapper;
const double EPSILON = 0.000000001;

TEST_CASE( "Eigen Transform to Transform 3D and Back", "[EigenTransformToTransform]" )
	{
	EigenTransformToTransform3DConverter firstConverter;
	Transform3DToEigenTransformConverter secondConverter;

	Eigen::Matrix4f inputTransform;
	Eigen::Quaternionf inputRotation(std::cos(M_PI/2), std::sin(M_PI/2), std::sin(M_PI/2)/2, std::sin(M_PI/2)*2);
	Eigen::Matrix3f eigenRotationMatrix = inputRotation.normalized().toRotationMatrix();
	inputTransform << 	eigenRotationMatrix(0,0), eigenRotationMatrix(0,1), eigenRotationMatrix(0,2), 0.3,
				eigenRotationMatrix(1,0), eigenRotationMatrix(1,1), eigenRotationMatrix(1,2), 0.4,
				eigenRotationMatrix(2,0), eigenRotationMatrix(2,1), eigenRotationMatrix(2,2), 0.6,
				0, 0, 0, 1;

	Transform3DSharedConstPtr asnTransform3D = firstConverter.ConvertShared(inputTransform);
	REQUIRE( GetXPosition(*asnTransform3D) < inputTransform(0, 3) + EPSILON );
	REQUIRE( GetXPosition(*asnTransform3D) > inputTransform(0, 3) - EPSILON );
	REQUIRE( GetYPosition(*asnTransform3D) < inputTransform(1, 3) + EPSILON );
	REQUIRE( GetYPosition(*asnTransform3D) > inputTransform(1, 3) - EPSILON );
	REQUIRE( GetZPosition(*asnTransform3D) < inputTransform(2, 3) + EPSILON );
	REQUIRE( GetZPosition(*asnTransform3D) > inputTransform(2, 3) - EPSILON );
	REQUIRE( GetXOrientation(*asnTransform3D) < inputRotation.normalized().x() + EPSILON );
	REQUIRE( GetXOrientation(*asnTransform3D) > inputRotation.normalized().x() - EPSILON );
	REQUIRE( GetYOrientation(*asnTransform3D) < inputRotation.normalized().y() + EPSILON );
	REQUIRE( GetYOrientation(*asnTransform3D) > inputRotation.normalized().y() - EPSILON );
	REQUIRE( GetZOrientation(*asnTransform3D) < inputRotation.normalized().z() + EPSILON );
	REQUIRE( GetZOrientation(*asnTransform3D) > inputRotation.normalized().z() - EPSILON );
	REQUIRE( GetWOrientation(*asnTransform3D) < inputRotation.normalized().w() + EPSILON );
	REQUIRE( GetWOrientation(*asnTransform3D) > inputRotation.normalized().w() - EPSILON );

	Eigen::Matrix4f outputTransform = secondConverter.ConvertShared(asnTransform3D);
	for(unsigned rowIndex = 0; rowIndex < 4; rowIndex++)
		{
		for(unsigned columnIndex = 0; columnIndex < 4; columnIndex++)
			{
			REQUIRE(outputTransform(rowIndex, columnIndex) < inputTransform(rowIndex, columnIndex) + EPSILON);
			REQUIRE(outputTransform(rowIndex, columnIndex) > inputTransform(rowIndex, columnIndex) - EPSILON);
			}
		}

	asnTransform3D.reset();
	}

TEST_CASE( "Transform3D to Eigen Transform and Back", "[Transform3DToEigenTransform]" )
	{
	EigenTransformToTransform3DConverter firstConverter;
	Transform3DToEigenTransformConverter secondConverter;

	Eigen::Matrix4f inputTransform;
	Eigen::Quaternionf inputRotation(std::cos(M_PI/2), std::sin(M_PI/2), std::sin(M_PI/2)/2, std::sin(M_PI/2)*2);
	Eigen::Matrix3f eigenRotationMatrix = inputRotation.normalized().toRotationMatrix();
	inputTransform << 	eigenRotationMatrix(0,0), eigenRotationMatrix(0,1), eigenRotationMatrix(0,2), 0.3,
				eigenRotationMatrix(1,0), eigenRotationMatrix(1,1), eigenRotationMatrix(1,2), 0.4,
				eigenRotationMatrix(2,0), eigenRotationMatrix(2,1), eigenRotationMatrix(2,2), 0.6,
				0, 0, 0, 1;

	Transform3DSharedConstPtr asnTransform3D = firstConverter.ConvertShared(inputTransform);
	Eigen::Matrix4f intermediateTransform = secondConverter.ConvertShared(asnTransform3D);
	Transform3DSharedConstPtr outputTransform = firstConverter.ConvertShared(intermediateTransform);

	REQUIRE( GetXPosition(*asnTransform3D) < GetXPosition(*outputTransform) + EPSILON );
	REQUIRE( GetXPosition(*asnTransform3D) > GetXPosition(*outputTransform) - EPSILON );
	REQUIRE( GetYPosition(*asnTransform3D) < GetYPosition(*outputTransform) + EPSILON );
	REQUIRE( GetYPosition(*asnTransform3D) > GetYPosition(*outputTransform) - EPSILON );
	REQUIRE( GetZPosition(*asnTransform3D) < GetZPosition(*outputTransform) + EPSILON );
	REQUIRE( GetZPosition(*asnTransform3D) > GetZPosition(*outputTransform) - EPSILON );
	REQUIRE( GetXOrientation(*asnTransform3D) < GetXOrientation(*outputTransform) + EPSILON );
	REQUIRE( GetXOrientation(*asnTransform3D) > GetXOrientation(*outputTransform) - EPSILON );
	REQUIRE( GetYOrientation(*asnTransform3D) < GetYOrientation(*outputTransform) + EPSILON );
	REQUIRE( GetYOrientation(*asnTransform3D) > GetYOrientation(*outputTransform) - EPSILON );
	REQUIRE( GetZOrientation(*asnTransform3D) < GetZOrientation(*outputTransform) + EPSILON );
	REQUIRE( GetZOrientation(*asnTransform3D) > GetZOrientation(*outputTransform) - EPSILON );
	REQUIRE( GetWOrientation(*asnTransform3D) < GetWOrientation(*outputTransform) + EPSILON );
	REQUIRE( GetWOrientation(*asnTransform3D) > GetWOrientation(*outputTransform) - EPSILON );

	asnTransform3D.reset();
	outputTransform.reset();
	}

TEST_CASE("Bad Transform conversion 1 (Eigen Transform)", "[Bad Transform 1]")
	{
	EigenTransformToTransform3DConverter firstConverter;

	Eigen::Matrix4f inputTransform1;
	inputTransform1 << 0.3, 0.2, 0.1, 0.4, 0.3, 0.2, 0.1, 0.1, 0.5, 0.3, 0.2, 0.1, 1, 0, 0, 1;
	REQUIRE_THROWS( firstConverter.ConvertShared(inputTransform1) );
	}

TEST_CASE("Bad Transform conversion 2 (Eigen Transform)", "[Bad Transform 2]")
	{
	EigenTransformToTransform3DConverter firstConverter;

	Eigen::Matrix4f inputTransform2;
	inputTransform2 << 0.3, 0.2, 0.1, 0.4, 0.3, 0.2, 0.1, 0.1, 0.5, 0.3, 0.2, 0.1, 0, 0, 0, 1;
	REQUIRE_THROWS( firstConverter.ConvertShared(inputTransform2) );
	}


//------------------------------------------------------------------------------------------
// to compile
//
// g++ -O4 mean_reversion.cpp -omean_rev
//
// this uses boost_1_47_0
// if boost is installed in /usr/include/boost_1_47_0
// with boost -> boost_1_47_0/boost the boost includes will be automatically picked up by g++
//------------------------------------------------------------------------------------------

#ifdef _USING_BOOST_
#include <boost/random/uniform_int.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>
#endif

#include <vector>
#include <algorithm>
#include <iostream>
#include <iomanip>
#include <numeric>
#include <math.h>

//------------------------------------------------------------------------------------------
// calcualte the mean, stdev, min and max
//------------------------------------------------------------------------------------------
void calc_stats( const std::vector<double> &v , double &mean, double &min_v , double &max_v , double &stdev)
{
  if( v.empty() )
    {
      mean=min_v=max_v=stdev=0;
      return;
    }
  mean = std::accumulate( v.begin() , v.end() , 0.0 ) / static_cast<double>(v.size());
  std::vector<double>::const_iterator itr;
  itr = std::min_element( v.begin() , v.end() );
  min_v = (*itr);
  itr = std::max_element( v.begin() , v.end() );
  max_v = (*itr);

  double sum = 0.0;
  for( size_t i = 0; i < v.size() ; i++ )
    {
      double d = v[i] - mean;
      d *= d;
      sum += d;
    }
  stdev = v.size() == 1 ? 0.0 : sqrt( sum / static_cast<double>(v.size()-1) );
}

//------------------------------------------------------------------------------------------
// print mean , min , max, range and stdev of randomly generated prices
//------------------------------------------------------------------------------------------
void print_stats( std::ostream &stream , const std::vector<double> &v , int draws , int tick_upper , int drift )
{
  double avg_mr , min_mr , max_mr , stdev_mr;
  calc_stats( v , avg_mr , min_mr , max_mr , stdev_mr );

  stream << std::setw(15)<< "mean reverting"
	 << " "
	 << std::setw(6) << draws
	 << " "
	 << std::setw(6) << tick_upper
	 << " "
	 << std::setw(6) << drift
	 << " "
	 << std::setw(10) << std::setprecision(2) << std::fixed << avg_mr
	 << " "
	 << std::setw(10) << std::setprecision(2) << std::fixed << min_mr
	 << " "
	 << std::setw(10) << std::setprecision(2) << std::fixed << max_mr
	 << " "
	 << std::setw(10) << std::setprecision(2) << std::fixed << (max_mr-min_mr)
	 << " "
	 << std::setw(10) << std::setprecision(2) << std::fixed << stdev_mr
	 << std::endl;
}

//------------------------------------------------------------------------------------------
// generate a uniform distribution of number in the range from lb to ub
//------------------------------------------------------------------------------------------
void gen_uniform( int lb , int ub , unsigned seed , int trials, std::vector<int> &rv )
{
  #ifdef _USING_BOOST
  boost::mt19937 random_num_gen( seed );
  boost::uniform_int<> urange(lb,ub);
  boost::variate_generator< boost::mt19937 , boost::uniform_int<> > uv( random_num_gen , urange );
  for( int i = 0 ; i < trials ; i++ ) rv.push_back( uv() );
  #else
  srand( seed );
  int range = ub - lb + 1;
  for( int i = 0 ; i < trials ; i++ ) rv.push_back( lb + ( rand() % range) );
  #endif
}

//------------------------------------------------------------------------------------------
// generate prices according to a random walk
// this is highly unstable and for illistrative pruposes only, can't be used
// as releastic source of mid price changes
//------------------------------------------------------------------------------------------
void random_walk( double start ,
		  double tick_size,
		  int draws ,
		  std::vector<double> &px )
{
  const unsigned  seed = 54897u;
  std::vector<int> rv;
  gen_uniform( -1, 1, seed , draws , rv );
  px.clear();
  px.push_back(start);
  px.reserve(draws);
  for( int i = 0; i < draws ; i++ )
    {
      double d = rv[i] == 0 ? 0 : ( rv[i] < 0 ? -1.0 : 1.0 ) ;
      px.push_back( px.back() + d * tick_size );
    }
}

//------------------------------------------------------------------------------------------
// the function generates random prices that have the following contraints:
// the sample of tick movements is constrained to -tick_upper to tick_upper+1
// the price chainge is biased towards the mean but allowed to drift away by drift ticks
// for drift
//------------------------------------------------------------------------------------------
void mean_reversion( double start ,
		     double tick_size ,
		     double mean ,
		     int    tick_upper ,
		     int    drift,
		     int    draws ,
		     std::vector<double> &px )
{
  const unsigned  seed = 54897u;
  std::vector<int> rv;
  gen_uniform( -tick_upper, tick_upper+1, seed , draws , rv );
  px.clear();
  px.push_back(start);
  px.reserve(draws);
  for( int i = 0; i < draws ; i++ )
    {
      int ticks = static_cast<int>((px.back() - mean) / tick_size);
      double delta = 0.0;
      if ( ( ticks < 0 && ticks + drift >= 0 ) ||
	   ( ticks > 0 && ticks - drift <= 0 ) )
	delta = rv[i] > 0 ? 1.0 : -1.0;
      else
	delta = rv[i] > ticks ? 1.0 : -1.0;
      px.push_back( px.back() + tick_size * delta );
    }

}

//---------------------------------------------------------------
// generate mean reverting prices for various tick ranges
// allowing for slight drift. the higher the drift the more
// variation there will be in prices, the higher the ticks the more
// variation there will be in prices
//---------------------------------------------------------------
int main( int , char ** )
{
  const double price      = 100.0;
  const double mean_price = 100.0;
  const double tick_size  = 0.01;
  const int    draws      = 100;
  const int    drift_div  = 2;

  for( int ticks =1000 ; ticks <= 1000 ; ticks += 100 )
    {
      std::vector<double> px;
      int drift = ticks / drift_div;
      mean_reversion( price , tick_size , mean_price , ticks , drift , draws , px );
      //print_stats(std::cout,px,draws,ticks,drift);
      for( size_t i = 0 ; i < px.size() ; i++ )
	std::cout << std::setw(10) << std::setprecision(3) << std::fixed << px[i] << std::endl;
    }
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/copy_const_reference.hpp>
#include <boost/python/return_internal_reference.hpp>
#include <scitbx/math/principal_axes_of_inertia.h>

namespace scitbx { namespace math {
namespace {

  struct principal_axes_of_inertia_wrappers
  {
    typedef principal_axes_of_inertia<> w_t;
    typedef principal_axes_of_inertia_2d<> w_t_2d;

    static void
    wrap()
    {
      using namespace boost::python;
      using boost::optional;
      typedef return_value_policy<copy_const_reference> ccr;
      typedef return_internal_reference<> rir;
      class_<w_t>("principal_axes_of_inertia", no_init)
        .def(init<af::const_ref<vec3<double> > const&,
                  optional<af::shared<double> > const&>(
          (arg("points"), arg("weights")=optional<af::shared<double> >())))
        .def("center_of_mass", &w_t::center_of_mass, ccr())
        .def("inertia_tensor", &w_t::inertia_tensor, ccr())
        .def("eigensystem", &w_t::eigensystem, rir())
        .def("change_of_basis_mx_to_principal",
          &w_t::change_of_basis_mx_to_principal)
        .def("distance_to_inertia_ellipsoid_surface",
          &w_t::distance_to_inertia_ellipsoid_surface, (
            arg("unit_direction")))
      ;
      class_<w_t_2d>("principal_axes_of_inertia_2d", no_init)
        .def(init<af::const_ref<vec2<double> > const&>((arg("points"))))
        .def(init<af::const_ref<vec2<double> > const&,
                  af::const_ref<double> const&>(
          (arg("points"), arg("weights"))))
        .def("center_of_mass", &w_t_2d::center_of_mass, ccr())
        .def("inertia_tensor", &w_t_2d::inertia_tensor, ccr())
        .def("eigensystem", &w_t_2d::eigensystem, rir())
        .def("distance_to_inertia_ellipsoid_surface",
          &w_t_2d::distance_to_inertia_ellipsoid_surface, (
            arg("unit_direction")))
      ;
    }
  };

} // namespace <anonymous>

namespace boost_python {

  void wrap_principal_axes_of_inertia()
  {
    using namespace boost::python;

    def("inertia_tensor",
      (sym_mat3<double>(*)(
        af::const_ref<vec3<double> > const&,
        vec3<double> const&)) inertia_tensor, (
          arg("points"), arg("pivot")));

    def("inertia_tensor",
      (sym_mat3<double>(*)(
        af::const_ref<vec3<double> > const&,
        af::const_ref<double> const&,
        vec3<double> const&)) inertia_tensor, (
          arg("points"), arg("weights"), arg("pivot")));

    principal_axes_of_inertia_wrappers::wrap();
  }

}}} // namespace scitbx::math::boost_python


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

#include <boost/hana/detail/sandbox/hash_map.hpp>

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

BOOST_HANA_CONSTEXPR_LAMBDA auto check_fold = [](auto ...pairs) {
    auto values = fmap(second, list(pairs...));
    auto result = foldr(hash_map(pairs...), list(), cons);
    BOOST_HANA_CONSTANT_ASSERT(elem(permutations(values), result));
};

template <int k, int v>
BOOST_HANA_CONSTEXPR_LAMBDA auto p = pair(int_<k>, int_<v>);

int main() {
    check_fold();
    check_fold(p<1, 1>);
    check_fold(p<1, 1>, p<2, 2>);
    check_fold(p<1, 1>, p<2, 2>, p<3, 3>);
    check_fold(p<1, 1>, p<2, 2>, p<3, 3>, p<4, 4>);
}


#include <Eigen/Dense>
#include <vector>
#include <iostream>

using Eigen::MatrixXd;

class NeuralLayer{
  MatrixXd _layer;

  NeuralLayer(int neurons_inputs, int neuronios){
    _layer = MatrixXd::Random(neurons_inputs, neuronios);
}

  public:
};