Copyright (c) 2017 Keiichiro KASHIWAGI

#ifndef ODE_MAFFINE_EXT_HPP
#define ODE_MAFFINE_EXT_HPP

#include <tuple>
#include <array>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/interval.hpp>
#include <kv/affine.hpp>
#include <kv/autodif.hpp>
#include <kv/ode-param.hpp>

#include "psa-ext.hpp"

namespace kv {
extern void *enabler;
namespace ub = boost::numeric::ublas;

template <int N, int E, bool F, typename T, typename... Seq> struct decl_psa_ext_impl;
template <int N, int E, typename T, typename... Seq>
struct decl_psa_ext_impl<N, E, false, T, Seq...> {
    typedef std::tuple<Seq...> type;
};
template <int N, int E, typename T, typename... Seq>
struct decl_psa_ext_impl<N, E, true, T, Seq...> {
    typedef typename decl_psa_ext_impl<N+1, E, N+1<E, T, psa_ext<N, T>, Seq...>::type type;
};
template <int S, int E, typename T> struct decl_psa_ext  {
    typedef typename decl_psa_ext_impl<S, E, true, T>::type type;
};

template <int N, typename... T, typename std::enable_if<N == sizeof...(T)-1>::type *& = enabler>
auto integral_impl(const std::tuple<T...>& x) {
    return std::make_tuple(integral(std::get<N>(x)));
}
template <int N, typename... T, typename std::enable_if<N != sizeof...(T)-1>::type *& = enabler>
auto integral_impl(const std::tuple<T...>& x) {
    return std::tuple_cat(std::make_tuple(integral(std::get<N>(x))), integral_impl<N+1>(x));
}
template <typename... T> auto integral(const std::tuple<T...>& x) {
    return integral_impl<0>(x);
}

template <int N, typename T, typename std::enable_if<N == std::tuple_size<T>::value>::type *& = enabler>
void initialize_impl(int n) {}
template <int N, typename T, typename std::enable_if<N != std::tuple_size<T>::value>::type *& = enabler>
void initialize_impl(int n) {
    std::tuple_element<N, T>::type ::init(n);
    initialize_impl<N+1, T>(n);
}
template <typename T> void initialize(int n) {
    initialize_impl<0, T>(n);
}

template <int N, typename T, typename R, typename std::enable_if<N == std::tuple_size<T>::value>::type *& = enabler>
void assign_impl(const R& r) {}
template <int N, typename T, typename R, typename std::enable_if<N != std::tuple_size<T>::value>::type *& = enabler>
void assign_impl(const R& r) {
    std::tuple_element<N, T>::type ::assign(r(N));
    assign_impl<N+1, T>(r);
}
template <typename T, typename R> void assign(const R& r) {
    assign_impl<0, T>(r);
}
template <int N, typename T, typename R, typename std::enable_if<N == std::tuple_size<T>::value>::type *& = enabler>
void assign_impl(int n) {}
template <int N, typename T, typename R, typename std::enable_if<N != std::tuple_size<T>::value>::type *& = enabler>
void assign_impl(int n) {
    std::tuple_element<N, T>::type ::template assign<typename std::tuple_element<N, R>::type>(n);
    assign_impl<N+1, T, R>(n);
}
template <typename T, typename R> void assign(int n) {
    assign_impl<0, T, R>(n);
}

template <int N, typename T, typename std::enable_if<N == std::tuple_size<T>::value>::type *& = enabler>
void eval_begin_impl() {}
template <int N, typename T, typename std::enable_if<N != std::tuple_size<T>::value>::type *& = enabler>
void eval_begin_impl() {
    std::tuple_element<N, T>::type ::eval_begin();
    eval_begin_impl<N+1, T>();
}
template <typename T> void eval_begin() {
    eval_begin_impl<0, T>();
}
template <int N, typename T, typename std::enable_if<N == std::tuple_size<T>::value>::type *& = enabler>
void eval_type1_impl(int n) {}
template <int N, typename T, typename std::enable_if<N != std::tuple_size<T>::value>::type *& = enabler>
void eval_type1_impl(int n) {
    std::tuple_element<N, T>::type ::eval_type1(n);
    eval_type1_impl<N+1, T>(n);
}
template <typename T> void eval_type1(int n) {
    eval_type1_impl<0, T>(n);
}
template <int N, typename T, typename Domain, typename std::enable_if<N == std::tuple_size<T>::value>::type *& = enabler>
void eval_type2_impl(const Domain& domain, int n) {}
template <int N, typename T, typename Domain, typename std::enable_if<N != std::tuple_size<T>::value>::type *& = enabler>
void eval_type2_impl(const Domain& domain, int n) {
    std::tuple_element<N, T>::type ::template eval_type2(domain, n);
    eval_type2_impl<N+1, T>(domain, n);
}
template <typename T, typename Domain> void eval_type2(const Domain& domain, int n) {
    eval_type2_impl<0, T>(domain, n);
}

template <int N, typename T, typename R1, typename R2, typename std::enable_if<N == std::tuple_size<T>::value>::type *& = enabler>
void copy_data_impl() {}
template <int N, typename T, typename R1, typename R2, typename std::enable_if<N != std::tuple_size<T>::value>::type *& = enabler>
void copy_data_impl() {
    std::tuple_element<N, T>::type ::template copy_from_autodif<typename std::tuple_element<N, R1>::type>();
    std::tuple_element<N, T>::type ::data[0] = std::tuple_element<N, R2>::type ::data[0].v;
    copy_data_impl<N+1, T, R1, R2>();
}
template <typename T, typename R1, typename R2> void copy_data() {
    copy_data_impl<0, T, R1, R2>();
}

template <int N, typename T, typename std::enable_if<N == std::tuple_size<T>::value-1>::type *& = enabler>
auto norm_max_impl(int n) {
    return std::abs(mid(std::tuple_element<N, T>::type ::v(n)));
}
template <int N, typename T, typename std::enable_if<N != std::tuple_size<T>::value-1>::type *& = enabler>
auto norm_max_impl(int n) {
    return std::max(std::abs(mid(std::tuple_element<N, T>::type ::v(n))), norm_max_impl<N+1, T>(n));
}
template <typename T> auto norm_max(int n) {
    return norm_max_impl<0, T>(n);
}

template <int N, typename T, typename R, typename std::enable_if<N == std::tuple_size<T>::value-1>::type *& = enabler>
auto rad_max_impl(int n) {
    return abs(std::tuple_element<N, T>::type ::v(n) - std::tuple_element<N, R>::type ::v(n)).upper();
}
template <int N, typename T, typename R, typename std::enable_if<N != std::tuple_size<T>::value-1>::type *& = enabler>
auto rad_max_impl(int n) {
    return std::max((abs(std::tuple_element<N, T>::type ::v(n) - std::tuple_element<N, R>::type ::v(n)).upper()), rad_max_impl<N+1, T, R>(n));
}
template <typename T, typename R> auto rad_max(int n) {
    return rad_max_impl<0, T, R>(n);
}

template <int N, typename T, typename R, typename std::enable_if<N == std::tuple_size<T>::value>::type *& = enabler>
void expand_impl(int n, const R& rad) {}
template <int N, typename T, typename R, typename std::enable_if<N != std::tuple_size<T>::value>::type *& = enabler>
void expand_impl(int n, const R& rad){
    std::tuple_element<N, T>::type ::v(n) += rad;
    expand_impl<N+1, T>(n, rad);
}
template <typename T, typename R> void expand(int n, const R& rad) {
    expand_impl<0, T>(n, rad);
}

template <int N, typename T, typename R, typename std::enable_if<N == std::tuple_size<T>::value-1>::type *& = enabler>
auto exist_test_impl(int n) {
    return subset(std::tuple_element<N, T>::type ::v(n), std::tuple_element<N, R>::type ::v(n));
}
template <int N, typename T, typename R, typename std::enable_if<N != std::tuple_size<T>::value-1>::type *& = enabler>
auto exist_test_impl(int n){
    if (! subset(std::tuple_element<N, T>::type ::v(n), std::tuple_element<N, R>::type ::v(n)))
        return false;
    return exist_test_impl<N+1, T, R>(n);
}
template <typename T, typename R> auto exist_test(int n) {
    return exist_test_impl<0, T, R>(n);
}

template <int N, typename C, typename Iad, typename E, typename Res, typename Init, typename D, typename std::enable_if<N == std::tuple_size<C>::value>::type *& = enabler>
void eval_result_impl(Res &result, const Init& init, const D& deltat, const D& deltat_n) {}
template <int N, typename C, typename Iad, typename E, typename Res, typename Init, typename D, typename std::enable_if<N != std::tuple_size<C>::value>::type *& = enabler>
void eval_result_impl(Res &result, const Init& init, const D& deltat, const D& deltat_n) {
    auto fc = eval_ext<typename std::tuple_element<N, C>::type>(deltat);       // f(c)
    auto fi_dif = eval_ext<typename std::tuple_element<N, Iad>::type>(deltat); // F'(I)
    auto tmp = fi_dif.d(0) * init(0);                                    // F'(I)*(v-c)
    for (int i=1; i<std::tuple_size<C>::value; ++i)
        tmp += fi_dif.d(i) * init(i);
    result(N) = fc + std::tuple_element<N, E>::type ::v(std::tuple_element<N, E>::type ::data.size()-1) * deltat_n + tmp;   // f(c) + F'(I)(v-c) + g(I)

    eval_result_impl<N+1, C, Iad, E>(result, init, deltat, deltat_n);
}
template <typename C, typename Iad, typename E, typename Res, typename Init, typename D>
void eval_result(Res &result, const Init& init, const D& deltat, const D& deltat_n) {
    eval_result_impl<0, C, Iad, E>(result, init, deltat, deltat_n);
}

template <int Dim, class F, class T>
int ode_maffine_ext(
        F f,
        ub::vector<affine<T>>& init,
        const interval<T>& start,
        interval<T>& end,
        ode_param<T> p = ode_param<T>()
) {
    ub::vector<interval<T>> c(Dim), I(Dim);
    ub::vector<autodif<interval<T>>> Iad;

    ub::vector<affine<T>>  result(Dim);

    int maxnum_save;

    affine<T> s1, s2;
    interval<T> s2i;
    ub::vector<affine<T>> s1_save;
    ub::vector<interval<T>> s2i_save;

    int ret_val;
    interval<T> end2, deltat;

    typedef time_ext<interval<T>> TIME_EXT;
    typedef typename decl_psa_ext<0, Dim, interval<T>>::type C_EXT;
    typedef typename decl_psa_ext<0, Dim, autodif<interval<T>>>::type Iad_EXT;
    typedef typename decl_psa_ext<Dim, Dim+Dim, interval<T>>::type E_EXT;

    TIME_EXT time_ext;
    C_EXT c_ext;
    Iad_EXT iad_ext;
    E_EXT e_ext;

    auto result_c_ext = integral(f(c_ext, time_ext));
    auto result_iad_ext = integral(f(iad_ext, time_ext));
    auto result_e_ext = integral(f(e_ext, time_ext));

    typedef decltype(result_c_ext) RESULT_C_EXT;
    typedef decltype(result_iad_ext) RESULT_Iad_EXT;
    typedef decltype(result_e_ext) RESULT_E_EXT;

    initialize<C_EXT>(p.order);
    initialize<Iad_EXT>(p.order);
    initialize<E_EXT>(p.order+1);

    initialize<RESULT_C_EXT>(p.order);
    initialize<RESULT_Iad_EXT>(p.order);
    initialize<RESULT_E_EXT>(p.order+1);

    for (int i=0; i<Dim; ++i) {
        I(i) = to_interval(init(i));
        c(i) = mid(I(i));
    }

    Iad = autodif<interval<T>>::init(I);

    TIME_EXT::assign(start);
    assign<C_EXT>(c);
    assign<Iad_EXT>(Iad);

    // Type1
    for (int i=0; i<p.order-1; ++i) {
        eval_begin<RESULT_C_EXT>();
        eval_begin<RESULT_Iad_EXT>();

        eval_type1<RESULT_C_EXT>(i);
        eval_type1<RESULT_Iad_EXT>(i);

        assign<C_EXT, RESULT_C_EXT>(i+1);
        assign<Iad_EXT, RESULT_Iad_EXT>(i+1);
    }

    copy_data<RESULT_E_EXT, RESULT_Iad_EXT, Iad_EXT>();

    eval_begin<RESULT_E_EXT>();
    eval_type1<RESULT_E_EXT>(p.order-1);
    assign<E_EXT, RESULT_E_EXT>(p.order);

    // Type2
#if 1
    T radius;
    int n_rad;
    T m = 1.;
    for (int i=0; i<Dim; i++) {
        m = std::max(m, norm(I(i)));
    }
    T tolerance = m * p.epsilon;

    if (p.autostep) {
        // use two non-zero coefficients of higher order term
        radius = 0.;
        n_rad = 0;
        for (int j = p.order; j>=1; j--) {
            m = norm_max<E_EXT>(j);
            if (m == 0.)
                continue;
            auto radius_tmp = std::pow((double)m, 1./j);
            // std::cout << j << " " << m << " " << radius_tmp << "\n";
            if (radius_tmp > radius)
                radius = radius_tmp;
            n_rad++;
            if (n_rad == 2)
                break;
        }
        radius = std::pow((double)tolerance, 1./p.order) / radius;
    }

    if (p.autostep) {
        end2 = mid(start + radius);
        if (end2 >= end.lower()) {
            end2 = end;
            ret_val = 2;
        } else
            ret_val = 1;
    } else {
        end2 = end;
        ret_val = 2;
    }
    deltat = end2 - start;
#endif

    auto domain = interval<T>::hull(0., deltat);
    eval_begin<RESULT_E_EXT>();
    eval_type2<RESULT_E_EXT>(domain, p.order);

    T rad = rad_max<RESULT_E_EXT, E_EXT>(p.order);
    rad *= 2.;
    expand<E_EXT>(p.order, interval<T>(-rad, rad));

    eval_begin<RESULT_E_EXT>();
    eval_type2<RESULT_E_EXT>(domain, p.order);

    if (! exist_test<RESULT_E_EXT, E_EXT>(p.order))
        return 0;

    if (p.ep_reduce == 0)
        maxnum_save = affine<T>::maxnum();

    // f(c) + F'(I)(v-c) + g(I)
    auto deltat_n = deltat;
    for (int i=0; i<p.order-1; ++i)
        deltat_n *= deltat;

    //v-c
    for (int i=0; i<Dim; ++i)
        init(i).a[0] = 0.;

    eval_result<C_EXT, Iad_EXT, RESULT_E_EXT>(result, init, deltat, deltat_n);

    if (p.ep_reduce == 0) {
        s1_save.resize(Dim);
        s2i_save.resize(Dim);
        for (int i=0; i<Dim; i++) {
            split(result(i), maxnum_save, s1, s2);
            s2i = to_interval(s2);
            s1_save(i) = s1;
            s2i_save(i) = s2i;
        }

        affine<T>::maxnum() = maxnum_save;
        for (int i=0; i<Dim; i++) {
            s1_save(i).resize();
            result(i) = append(s1_save(i), (affine<T>)s2i_save(i));
        }
    } else
        epsilon_reduce(result, p.ep_reduce, p.ep_reduce_limit);

    init = result;
    if (ret_val == 1)
        end = end2;

    return ret_val;
}

template <int Dim, class F, class T>
int odelong_maffine_ext(
    F f,
    ub::vector<affine<T>>& init,
    const interval<T>& start,
    interval<T>& end,
    ode_param<T> p = ode_param<T>(),
    ub::matrix<interval<T>>* mat = nullptr
) {
    ub::vector<affine<T>> x, x1;
    interval<T> t, t1;
    int r;
    int ret_val = 0;

    x = init;
    t = start;
    p.set_autostep(true);
    while (1) {
        x1 = x;
        t1 = end;

        r = ode_maffine_ext<Dim>(f, x1, t, t1, p);
        if (r == 0) {
            if (ret_val == 1) {
                init = x1;
                end = t;
            }
            return ret_val;
        }
        ret_val = 1;
        if (p.verbose == 1) {
            std::cout << "t: " << t1 << "\n";
            std::cout << to_interval(x1) << "\n";
        }

        if (r == 2) {
            init = x1;
            return 2;
        }
        t = t1;
        x = x1;
    }
}

template <int Dim, class F, class T>
int odelong_maffine_ext(
    F f,
    ub::vector<interval<T>>& init,
    const interval<T>& start,
    interval<T>& end,
    ode_param<T> p = ode_param<T>()
) {
    auto end2 = end;

    int maxnum_save = affine<T>::maxnum();
    affine<T>::maxnum() = 0;

    ub::vector<affine<T>> x = init;

    int r = odelong_maffine_ext<Dim>(f, x, start, end2, p);

    affine<T>::maxnum() = maxnum_save;

    if (r == 0)
        return 0;

    for (int i=0; i<(int)init.size(); ++i)
        init(i) = to_interval(x(i));
    if (r == 1)
        end = end2;

    return r;
}
template <unsigned long N, class F, class T>
int odelong_maffine_ext(
    F f,
    std::array<interval<T>, N>& init,
    const interval<T>& start,
    interval<T>& end,
    ode_param<T> p = ode_param<T>()
)  {
    ub::vector<interval<T>> init2(N);
    for (int i=0; i<N; ++i)
        init2(i) = init[i];
    int r = odelong_maffine_ext<N>(f, init2, start, end, p);
    for (int i=0; i<N; ++i)
        init[i] = init2(i);
    return r;
}
template <typename T, typename... R>
auto make_init(const R&... x) {
    return std::array<T, sizeof...(R)> { T(x)... };
}

} // namespace kv

#endif


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * testDiscreteBayesNet.cpp
 *
 *  @date Feb 27, 2011
 *  @author Frank Dellaert
 */

#include <gtsam/discrete/DiscreteBayesNet.h>
#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/debug.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/map.hpp>
#include <boost/assign/list_inserter.hpp>

using namespace boost::assign;

#include <iostream>

using namespace std;
using namespace gtsam;

/* ************************************************************************* */
TEST(DiscreteBayesNet, Asia)
{
  DiscreteBayesNet asia;
//  DiscreteKey A("Asia"), S("Smoking"), T("Tuberculosis"), L("LungCancer"), B(
//      "Bronchitis"), E("Either"), X("XRay"), D("Dyspnoea");
  DiscreteKey A(0,2), S(4,2), T(3,2), L(6,2), B(7,2), E(5,2), X(2,2), D(1,2);

  // TODO: make a version that doesn't use the parser
  asia.add(A % "99/1");
  asia.add(S % "50/50");

  asia.add(T | A = "99/1 95/5");
  asia.add(L | S = "99/1 90/10");
  asia.add(B | S = "70/30 40/60");

  asia.add((E | T, L) = "F T T T");

  asia.add(X | E = "95/5 2/98");
  // next lines are same as asia.add((D | E, B) = "9/1 2/8 3/7 1/9");
  DiscreteConditional::shared_ptr actual =
      boost::make_shared<DiscreteConditional>((D | E, B) = "9/1 2/8 3/7 1/9");
  asia.push_back(actual);
  //  GTSAM_PRINT(asia);

  // Convert to factor graph
  DiscreteFactorGraph fg(asia);
//    GTSAM_PRINT(fg);
  LONGS_EQUAL(3,fg.back()->size());
  Potentials::ADT expected(B & D & E, "0.9 0.3 0.1 0.7 0.2 0.1 0.8 0.9");
  CHECK(assert_equal(expected,(Potentials::ADT)*actual));

  // Create solver and eliminate
  Ordering ordering;
  ordering += Key(0),Key(1),Key(2),Key(3),Key(4),Key(5),Key(6),Key(7);
  DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering);
//    GTSAM_PRINT(*chordal);
  DiscreteConditional expected2(B % "11/9");
  CHECK(assert_equal(expected2,*chordal->back()));

  // solve
  DiscreteFactor::sharedValues actualMPE = chordal->optimize();
  DiscreteFactor::Values expectedMPE;
  insert(expectedMPE)(A.first, 0)(D.first, 0)(X.first, 0)(T.first, 0)(S.first,
      0)(E.first, 0)(L.first, 0)(B.first, 0);
  EXPECT(assert_equal(expectedMPE, *actualMPE));

  // add evidence, we were in Asia and we have Dispnoea
  fg.add(A, "0 1");
  fg.add(D, "0 1");
//  fg.product().dot("fg");

  // solve again, now with evidence
  DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering);
//  GTSAM_PRINT(*chordal2);
  DiscreteFactor::sharedValues actualMPE2 = chordal2->optimize();
  DiscreteFactor::Values expectedMPE2;
  insert(expectedMPE2)(A.first, 1)(D.first, 1)(X.first, 0)(T.first, 0)(S.first,
      1)(E.first, 0)(L.first, 0)(B.first, 1);
  EXPECT(assert_equal(expectedMPE2, *actualMPE2));

  // now sample from it
  DiscreteFactor::Values expectedSample;
  SETDEBUG("DiscreteConditional::sample", false);
  insert(expectedSample)(A.first, 1)(D.first, 1)(X.first, 1)(T.first, 0)(
      S.first, 1)(E.first, 1)(L.first, 1)(B.first, 0);
  DiscreteFactor::sharedValues actualSample = chordal2->sample();
  EXPECT(assert_equal(expectedSample, *actualSample));
}

/* ************************************************************************* */
TEST_UNSAFE(DiscreteBayesNet, Sugar)
{
  DiscreteKey T(0,2), L(1,2), E(2,2), D(3,2), C(8,3), S(7,2);

  DiscreteBayesNet bn;

  // test some mistakes
  //  add(bn, D);
  //  add(bn, D | E);
  //  add(bn, D | E = "blah");

  // try logic
  bn.add((E | T, L) = "OR");
  bn.add((E | T, L) = "AND");

  //  // try multivalued
 bn.add(C % "1/1/2");
 bn.add(C | S = "1/1/2 5/2/3");
}

/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/complement.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/function/shift_left.hpp>

STF_CASE_TPL ("check complement for floating",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::complement;
  using bs::tag::complement_;
  using r_t = decltype(complement(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(complement(bs::Nan<T>()), bs::Zero<r_t>());
#endif
  STF_IEEE_EQUAL(complement(bs::Zero<T>()), bs::Nan<r_t>());
} // end of test for floating_

STF_CASE_TPL ("check complement for integral",  STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::complement;
  using bs::tag::complement_;
  using r_t = decltype(complement(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(complement(bs::One<T>()), bs::shift_left(bs::Mone<r_t>(),1));
  STF_EQUAL(complement(bs::Three<T>()), bs::shift_left(bs::Mone<r_t>(),2));
  STF_EQUAL(complement(bs::Zero<T>()), bs::Mone<r_t>());
} // end of test for integer_


/*
    Copyright (c) 2017 Mobile Robots Laboratory at Poznan University of Technology:
    -Jan Wietrzykowski name.surname [at] put.poznan.pl

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/

#include <iostream>
#include <random>
#include <algorithm>
#include <vector>
#include <map>
#include <chrono>
#include <thread>
#include <queue>
#include <cmath>

#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/supervoxel_clustering.h>
#include <pcl/impl/point_types.hpp>

#include <Eigen/Eigen>
#include <pcl/features/integral_image_normal.h>

#include "PlaneSegmentation.hpp"
#include "Misc.hpp"
#include "Exceptions.hpp"

using namespace std;

int PlaneSegmentation::curObjInstId = 0;

void PlaneSegmentation::segment(const cv::FileStorage& fs,
						pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcNormals,
						pcl::PointCloud<pcl::PointXYZRGBL>::Ptr pcLab,
						vectorObjInstance& objInstances,
						bool segmentMap,
						pcl::visualization::PCLVisualizer::Ptr viewer,
						int viewPort1,
						int viewPort2)
{
	cout << "Segmentation::segment" << endl;
	chrono::high_resolution_clock::time_point startTime = chrono::high_resolution_clock::now();

	// parameters
//	float planeDistThresh = (double)fs["segmentation"]["planeDistThresh"];
//	float areaMult = (double)fs["segmentation"]["areaMult"];
//	float smoothCost = (double)fs["segmentation"]["smoothCost"];
//	float minNumInlier = (double)fs["segmentation"]["minNumInlier"];
    float curvThresh = (double)fs["segmentation"]["curvThresh"];
    float stepThresh = (double)fs["segmentation"]["stepThresh"];
    float areaThresh = (double)fs["segmentation"]["areaThresh"];
    float normalThresh = (double)fs["segmentation"]["normalThresh"];

    vector<PlaneSeg, Eigen::aligned_allocator<PlaneSeg>> svsInfo;

    makeSupervoxels(fs,
                    pcNormals,
                    segmentMap,
                    svsInfo);

    UnionFind sets(svsInfo.size());

    vector<PlaneSeg, Eigen::aligned_allocator<PlaneSeg>> planeSegs = svsInfo;

    mergeSegmentsFF(planeSegs,
                    sets,
                    curvThresh,
                    normalThresh,
                    stepThresh,
                    viewer,
                    viewPort1,
                    viewPort2);

    vector<int> svLabels(svsInfo.size(), -1);
	makeObjInstances(svsInfo,
                     planeSegs,
                     sets,
                     svLabels,
                     pcLab,
                     objInstances,
                     curvThresh,
                     normalThresh,
                     stepThresh,
                     areaThresh,
                     viewer,
                     viewPort1,
                     viewPort2);

	if(viewer){
		cout << "displaying" << endl;
//		pcl::PointCloud<pcl::PointXYZL>::Ptr pcLabSegm = svClust.getLabeledVoxelCloud();
        visualizeSegmentation(svsInfo,
                             planeSegs,
                             svLabels,
                             viewer,
                             viewPort1,
                             viewPort2);
	}

//	if(pcCol != nullptr){
//		pcCol = svClust.getColoredVoxelCloud();
//	}

    chrono::high_resolution_clock::time_point endTime = chrono::high_resolution_clock::now();

    static chrono::milliseconds totalTime = chrono::milliseconds::zero();
    static int totalCnt = 0;

    if(!segmentMap) {
        totalTime += chrono::duration_cast<chrono::milliseconds>(endTime - startTime);
        ++totalCnt;

        cout << "Mean segmentation time: " << (totalTime.count() / totalCnt) << endl;
    }
}

void PlaneSegmentation::segment(const cv::FileStorage &fs,
                                cv::Mat rgb,
                                cv::Mat depth,
                                pcl::PointCloud<pcl::PointXYZRGBL>::Ptr pcLab,
                                vectorObjInstance &objInstances,
                                pcl::visualization::PCLVisualizer::Ptr viewer,
                                int viewPort1,
                                int viewPort2)
{
    cout << "Segmentation::segment" << endl;
    chrono::high_resolution_clock::time_point startTime = chrono::high_resolution_clock::now();

    // parameters
//	float planeDistThresh = (double)fs["segmentation"]["planeDistThresh"];
//	float areaMult = (double)fs["segmentation"]["areaMult"];
//	float smoothCost = (double)fs["segmentation"]["smoothCost"];
//	float minNumInlier = (double)fs["segmentation"]["minNumInlier"];
    float curvThresh = (double)fs["segmentation"]["curvThresh"];
    float stepThresh = (double)fs["segmentation"]["stepThresh"];
    float areaThresh = (double)fs["segmentation"]["areaThresh"];
    float normalThresh = (double)fs["segmentation"]["normalThresh"];

    vector<PlaneSeg, Eigen::aligned_allocator<PlaneSeg>> svsInfo;

    makeSupervoxels(fs,
                    rgb,
                    depth,
                    svsInfo);

    UnionFind sets(svsInfo.size());

//    if(viewer) {
//        drawSegments(viewer,
//                     "cloud_svs",
//                     viewPort1,
//                     svsInfo,
//                     sets);
//
//        viewer->resetStoppedFlag();
//        viewer->initCameraParameters();
//        viewer->setCameraPosition(0.0, 0.0, -6.0, 0.0, 1.0, 0.0);
//        viewer->spinOnce(100);
//        while (!viewer->wasStopped()) {
//            viewer->spinOnce(50);
//            cv::waitKey(50);
//            std::this_thread::sleep_for(std::chrono::milliseconds(50));
//        }
//
//        viewer->removePointCloud("cloud_svs", viewPort1);
//    }

    vector<PlaneSeg, Eigen::aligned_allocator<PlaneSeg>> planeSegs = svsInfo;

    mergeSegmentsFF(planeSegs,
                    sets,
                    curvThresh,
                    normalThresh,
                    stepThresh/*,
                    viewer,
                    viewPort1,
                    viewPort2*/);

    vector<int> svLabels(svsInfo.size(), -1);
    makeObjInstances(svsInfo,
                     planeSegs,
                     sets,
                     svLabels,
                     pcLab,
                     objInstances,
                     curvThresh,
                     normalThresh,
                     stepThresh,
                     areaThresh,
                     viewer,
                     viewPort1,
                     viewPort2);

    if(viewer){
        cout << "displaying" << endl;
//		pcl::PointCloud<pcl::PointXYZL>::Ptr pcLabSegm = svClust.getLabeledVoxelCloud();
        visualizeSegmentation(svsInfo,
                              planeSegs,
                              svLabels,
                              viewer,
                              viewPort1,
                              viewPort2);
    }

//	if(pcCol != nullptr){
//		pcCol = svClust.getColoredVoxelCloud();
//	}

    chrono::high_resolution_clock::time_point endTime = chrono::high_resolution_clock::now();

    static chrono::milliseconds totalTime = chrono::milliseconds::zero();
    static int totalCnt = 0;

    totalTime += chrono::duration_cast<chrono::milliseconds>(endTime - startTime);
    ++totalCnt;

    cout << "Mean segmentation time: " << (totalTime.count() / totalCnt) << endl;
}

void
PlaneSegmentation::makeSupervoxels(const cv::FileStorage &fs,
                                  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcNormals,
                                  bool segmentMap,
                                  std::vector<PlaneSeg, Eigen::aligned_allocator<PlaneSeg>> &svs)
{
    // compute normals
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    {
        for(int p = 0; p < pcNormals->size(); ++p){
            pcl::Normal norm;
            norm.normal_x = pcNormals->at(p).normal_x;
            norm.normal_y = pcNormals->at(p).normal_y;
            norm.normal_z = pcNormals->at(p).normal_z;
//            cout << "norm.norm() = " << norm.getNormalVector3fMap().transpose() << endl;
            normals->push_back(norm);
        }
    }
//	{
//		  // Create the normal estimation class, and pass the input dataset to it
//		  pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
//		  ne.setInputCloud(pc);
//
//		  // Create an empty kdtree representation, and pass it to the normal estimation object.
//		  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
//		  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB> ());
//		  ne.setSearchMethod(tree);
//
//		  // Use all neighbors in a sphere of radius 3cm
//		  ne.setRadiusSearch(0.03);
//
//		  // Compute the features
//		  ne.compute(*pointCloudNorm);
//
//	}
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc(new pcl::PointCloud<pcl::PointXYZRGB>());
    for(int p = 0; p < pcNormals->size(); ++p){
        pcl::PointXYZRGB pt;
        pt.x = pcNormals->at(p).x;
        pt.y = pcNormals->at(p).y;
        pt.z = pcNormals->at(p).z;
        pt.r = pcNormals->at(p).r;
        pt.g = pcNormals->at(p).g;
        pt.b = pcNormals->at(p).b;
        pc->push_back(pt);
    }

    double svRes = 0.006;
    double svSize = 0.2;
    bool useSingleCameraTrans = true;
    if (segmentMap) {
        svRes = 0.01;
        svSize = 0.2;
        useSingleCameraTrans = false;
    }

    cout << "Creating supervoxels" << endl;
    map<uint32_t, pcl::Supervoxel<pcl::PointXYZRGB>::Ptr> idToSv;
    pcl::SupervoxelClustering<pcl::PointXYZRGB> svClust(svRes, svSize);
    svClust.setUseSingleCameraTransform(useSingleCameraTrans);
    svClust.setInputCloud(pc);
    svClust.setNormalCloud(normals);
    svClust.setColorImportance(1.0);
    svClust.setSpatialImportance(0.4);
    svClust.setNormalImportance(1.0);
//	svClust.set
    svClust.extract(idToSv);

    svClust.refineSupervoxels(1, idToSv);

    vector<uint32_t> idxToId;
    map<uint32_t, uint32_t> idToIdx;
    int idxCnt = 0;
    for(auto it = idToSv.begin(); it != idToSv.end(); ++it){
        // ignore label 0
        if(it->first != 0) {
            idxToId.push_back(it->first);
            idToIdx[it->first] = idxCnt;
//                cout << "idToIdx[" << it->first << "] = " << idxCnt << endl;
            ++idxCnt;
        }
    }

    cout << "Adding points to svsInfo" << endl;
    svs.resize(idxCnt);

    pcl::PointCloud<pcl::PointXYZL>::Ptr pcLabSegm = svClust.getLabeledCloud();
    for(int p = 0; p < pcLabSegm->size(); ++p){
        // ignore label 0
        int svId = pcLabSegm->at(p).label;
        if(svId != 0) {
            if (idToIdx.count(svId) > 0) {
                int svIdx = idToIdx.at(svId);
//                cout << "svId = " << svId << ", svIdx = " << svIdx << endl;
                svs[svIdx].addPointAndNormal(pc->at(p), normals->at(p));
            } else {
                cout << "svId = " << svId << " not found!!!" << endl;
            }
        }
    }

    cout << "Calculating segment properties" << endl;
    for(int sv = 0; sv < svs.size(); ++sv){
        svs[sv].setId(sv);
        svs[sv].setLabel(0);
        svs[sv].calcSegProp();
    }

    cout << "Adding edges" << endl;
    std::multimap<uint32_t, uint32_t> adjMultimap;
    svClust.getSupervoxelAdjacency(adjMultimap);
    for(auto it = adjMultimap.begin(); it != adjMultimap.end(); ++it) {
        int srcIdx = idToIdx[it->first];
        int tarIdx = idToIdx[it->second];

        svs[srcIdx].addAdjSeg(tarIdx);
        svs[tarIdx].addAdjSeg(srcIdx);
    }
}

void PlaneSegmentation::makeSupervoxels(const cv::FileStorage &fs,
                                        cv::Mat rgb,
                                        cv::Mat depth,
                                        std::vector<PlaneSeg, Eigen::aligned_allocator<PlaneSeg>> &svs)
{
    cv::Mat camMat;
    fs["planeSlam"]["cameraMatrix"] >> camMat;
    double curvThresh = fs["segmentation"]["curvThresh"];

    cv::Mat rgbSegments = segmentRgb(rgb, depth, 0.8, 20, 200);

    int nrows = rgbSegments.rows;
    int ncols = rgbSegments.cols;

    {
        cv::Mat filtRgbSegments = rgbSegments.clone();

        int kSize = 5;
        // majority filter
        for(int r = 0; r < nrows; ++r) {
            for (int c = 0; c < ncols; ++c) {
                if(rgbSegments.at<int>(r, c) >= 0) {
                    map<int, int> votes;
                    for (int dr = -kSize / 2; dr <= kSize / 2; ++dr) {
                        for (int dc = -kSize / 2; dc <= kSize / 2; ++dc) {
                            int cr = r + dr;
                            int cc = c + dc;
                            if ((cr < nrows) && (cr >= 0) &&
                                (cc < ncols) && (cc >= 0))
                            {
                                int cid = rgbSegments.at<int>(cr, cc);
                                if(votes.count(cid) == 0){
                                    votes[cid] = 1;
                                }
                                else{
                                    ++votes[cid];
                                }
                            }
                        }
                    }
                    int bestId = -1;
                    int bestVotes = -1;
                    for(auto &p : votes){
                        if(p.second > bestVotes){
                            bestVotes = p.second;
                            bestId = p.first;
                        }
                    }
//                    if(bestId != )
                    filtRgbSegments.at<int>(r, c) = bestId;
                }
            }
        }

        rgbSegments = filtRgbSegments;
    }

    int nhood[][2] = {{-1, 1},
                      {1, 0},
                      {1, 1},
                      {0, 1}};

    int maxId = 0;
    map<pair<int, int>, int> edges;
    for(int r = 0; r < nrows; ++r){
        for(int c = 0; c < ncols; ++c){
            int id = rgbSegments.at<int>(r, c);

            maxId = std::max(maxId, id);

            for(int nh = 0; nh < sizeof(nhood)/sizeof(nhood[0]); nh++){
                int nhr = r + nhood[nh][0];
                int nhc = c + nhood[nh][1];

                if((nhr < nrows) && (nhr >= 0) &&
                   (nhc < ncols) && (nhc >= 0) &&
                    rgbSegments.at<uint8_t>(r, c) >= 0 &&
                    rgbSegments.at<uint8_t>(nhr, nhc) >= 0)
                {
                    int nhId = rgbSegments.at<int>(nhr, nhc);

                    if(nhId != id) {
                        pair<int, int> e = make_pair(std::min(id, nhId), std::max(id, nhId));
                        edges[e] += 1;
                    }
                }
            }
        }
    }

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZRGB>(rgb.cols, rgb.rows));
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>(rgb.cols, rgb.rows));
    cv::Mat xyz = Misc::projectTo3D(depth, camMat);

    for(int r = 0; r < rgb.rows; ++r) {
        for (int c = 0; c < rgb.cols; ++c) {
            pcl::PointXYZRGB p;
//					((uint8_t)rgb.at<Vec3b>(r, c)[0],
//										(uint8_t)rgb.at<Vec3b>(r, c)[1],
//										(uint8_t)rgb.at<Vec3b>(r, c)[2]);
            p.x = xyz.at<cv::Vec3f>(r, c)[0];
            p.y = xyz.at<cv::Vec3f>(r, c)[1];
            p.z = xyz.at<cv::Vec3f>(r, c)[2];
            p.r = (uint8_t) rgb.at<cv::Vec3b>(r, c)[0];
            p.g = (uint8_t) rgb.at<cv::Vec3b>(r, c)[1];
            p.b = (uint8_t) rgb.at<cv::Vec3b>(r, c)[2];
            //					cout << "Point at (" << xyz.at<Vec3f>(r, c)[0] << ", " <<
            //											xyz.at<Vec3f>(r, c)[1] << ", " <<
            //											xyz.at<Vec3f>(r, c)[2] << "), rgb = (" <<
            //											(int)rgb.at<Vec3b>(r, c)[0] << ", " <<
            //											(int)rgb.at<Vec3b>(r, c)[1] << ", " <<
            //											(int)rgb.at<Vec3b>(r, c)[2] << ") " << endl;
            pointCloud->at(c, r) = p;
        }
    }

    svs.resize(maxId + 1);

    pcl::IntegralImageNormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
    ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
    ne.setMaxDepthChangeFactor(0.02f);
    ne.setNormalSmoothingSize(20.0f);
    ne.setInputCloud(pointCloud);
    ne.setViewPoint(0.0, 0.0, 0.0);

    ne.compute(*normals);

    for(int r = 0; r < nrows; ++r) {
        for (int c = 0; c < ncols; ++c) {
            int id = rgbSegments.at<int>(r, c);

            const pcl::PointXYZRGB &curPt = pointCloud->at(c, r);
            const pcl::Normal &curNorm = normals->at(c, r);

            if(id >= 0 &&
               !isnan(curPt.x) && !isnan(curPt.y) && !isnan(curPt.z) &&
               abs(curPt.z) >= 1e-3 &&
               !isnan(curNorm.normal_x) && !isnan(curNorm.normal_y) && !isnan(curNorm.normal_z))
            {
                svs[id].addPointAndNormal(pointCloud->at(c, r), normals->at(c, r));
            }
        }
    }

    {
        map<int, int> oldIdxToNewIdx;
        std::vector<PlaneSeg, Eigen::aligned_allocator<PlaneSeg>> newSvs;
        int svsCnt = 0;
        for(int sv = 0; sv < svs.size(); ++sv){
            svs[sv].calcSegProp(true);

            if(svs[sv].getPoints()->size() > 50) {
                if (!isnan(svs[sv].getSegCurv()) &&
                    svs[sv].getSegCurv() < 2 * curvThresh)
                {
                    const vector<double> &evals = svs[sv].getEvals();

                    double varD = evals[2] / svs[sv].getPoints()->size();
                    double varX = evals[2] / evals[0];
                    double varY = evals[2] / evals[1];
//                    if(evals.size() < 3){
//                        throw PLANE_EXCEPTION("evals.size() < 3");
//                    }
                    if (varX < 0.4 && varY < 0.4) {

                        int newIdx = svsCnt++;

                        newSvs.push_back(svs[sv]);
                        newSvs.back().setId(newIdx);
                        oldIdxToNewIdx[sv] = newIdx;
                    }
                }
            }
        }
        svs.swap(newSvs);

        map<pair<int, int>, int> newEdges;
        for(const pair<pair<int, int>, int> &cure : edges){
            if(oldIdxToNewIdx.count(cure.first.first) > 0 &&
               oldIdxToNewIdx.count(cure.first.second) > 0)
            {
               newEdges.insert(make_pair(make_pair(oldIdxToNewIdx[cure.first.first], oldIdxToNewIdx[cure.first.second]), cure.second));
            }
        }
        edges.swap(newEdges);
    }

    for(const pair<pair<int, int>, int> &cure : edges){
        // if edge is strong enough than add it to svs
        static constexpr int edgeStrengthThresh = 50;
        if(cure.second > edgeStrengthThresh) {
            int u = cure.first.first;
            int v = cure.first.second;
            svs[u].addAdjSeg(v);
            svs[v].addAdjSeg(u);
        }
    }
//    for(int sv = 0; sv < svs.size(); ++sv){
//        svs[sv].calcSegProp();
//    }

//    cv::Mat segCol = Misc::colorIdsWithLabels(rgbSegments);
    cv::Mat segCol = Misc::colorIds(rgbSegments);

    cv::Mat bgr;
    cv::cvtColor(rgb, bgr, cv::COLOR_RGB2BGR);
    cv::imshow("original", bgr);
    cv::imshow("rgb segments", segCol);
}

void PlaneSegmentation::makeObjInstances(const vectorPlaneSeg &svs,
                                         const vectorPlaneSeg &segs,
                                         UnionFind &sets,
                                         std::vector<int> &svLabels,
                                         pcl::PointCloud<pcl::PointXYZRGBL>::Ptr pcLab,
                                         vectorObjInstance &objInstances,
                                         float curvThresh,
                                         double normalThresh,
                                         double stepThresh,
                                         float areaThresh,
                                         pcl::visualization::PCLVisualizer::Ptr viewer,
                                         int viewPort1,
                                         int viewPort2)
{
    cout << "making obj instances" << endl;

    vector<vector<int> > planeCandidates(svs.size());
    vector<int> planeCandidatesNpts(svs.size(), 0);
    vector<double> planeCandidatesAreaEst(svs.size(), 0.0);
    for(int sv = 0; sv < svs.size(); ++sv){
        int set = sets.findSet(sv);
//            svLabels[sv] = set;
        planeCandidates[set].push_back(sv);
        planeCandidatesNpts[set] += svs[sv].getPoints()->size();
        planeCandidatesAreaEst[set] += svs[sv].getAreaEst();
    }

    for(int pl = 0; pl < segs.size(); ++pl){
//        cout << "planeCandidates[" << pl << "].size() = " << planeCandidates[pl].size() << endl;
//        cout << "planeCandidatesNpts[" << pl << "] = " << planeCandidatesNpts[pl] << endl;
////        cout << "planeCandidatesAreaEst[" << pl << "] = " << planeCandidatesAreaEst[pl] << endl;
//        cout << "planeSegs[" << pl << "].getSegCurv() = " << segs[pl].getSegCurv() << endl;

        if(planeCandidates[pl].size() > 0){
            if(planeCandidatesNpts[pl] > 1500){
                if(segs[pl].getSegCurv() < curvThresh){
//                    cout << "Adding with lab = " << pl << endl;
                    int lab = pl;

                    vectorPlaneSeg curSvs;
//                    pcl::PointCloud<pcl::PointXYZRGB>::Ptr curPoints(new pcl::PointCloud<pcl::PointXYZRGB>());

                    for(int s = 0; s < planeCandidates[pl].size(); ++s){
                        curSvs.push_back(svs[planeCandidates[pl][s]]);
                        svLabels[planeCandidates[pl][s]] = lab;
                    }
//                        cout << "svLabels[pl] = " << svLabels[pl] << endl;
//                        cout << "planeSegs[pl].getPoints()->size() = " << planeSegs[pl].getPoints()->size() << endl;

                    pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr curPoints = segs[pl].getPoints();
//                    *curPoints = *(segs[pl].getPoints());
                    for(int p = 0; p < curPoints->size(); ++p){
                        const pcl::PointXYZRGB &pcPt = curPoints->at(p);
                        pcl::PointXYZRGBL pt;
                        pt.rgb = pcPt.rgb;
                        pt.x = pcPt.x;
                        pt.y = pcPt.y;
                        pt.z = pcPt.z;
                        pt.label = lab;
                        pcLab->push_back(pt);
                    }

                    objInstances.emplace_back(curObjInstId++,
                                              ObjInstance::ObjType::Plane,
                                              curPoints,
                                              curSvs);
                    {
//                        Eigen::Vector4d planeEq = objInstances.back().getNormal();
//                        Eigen::Vector3d centroid = objInstances.back().getCentroid();
                        // the area has to greater than threshold
                        if(objInstances.back().getHull().getTotalArea() < areaThresh)
                        {
//                            cout << "removing, area = " << objInstances.back().getHull().getTotalArea() << endl;
                            // remove last element
                            objInstances.erase(objInstances.end() - 1);
                        }
                    }
                }
            }
        }
    }
}

cv::Mat
PlaneSegmentation::segmentRgb(cv::Mat rgb, cv::Mat depth, float sigma, float k, int minSegment)
{
    using namespace std::chrono;
    high_resolution_clock::time_point start = high_resolution_clock::now();
    high_resolution_clock::time_point endSorting;
    high_resolution_clock::time_point endComp;
    high_resolution_clock::time_point endMerging;

    cv::Mat mask = (depth > 0.2) & (depth < 4.0);

//    cv::Mat imageR(rgb.rows, rgb.cols, CV_32FC1);
//    cv::Mat imageG(rgb.rows, rgb.cols, CV_32FC1);
//    cv::Mat imageB(rgb.rows, rgb.cols, CV_32FC1);
//    cv::Mat imageChannels[] = {imageR, imageG, imageB};
    cv::Mat imageFilt(rgb.rows, rgb.cols, CV_32FC3);
    cv::Mat depthFilt = depth.clone();

//    int nchannels = 3;
    int nhood[][2] = {{-1, 1},
                      {1, 0},
                      {1, 1},
                      {0, 1}};
    /*int nhood[][2] = {{1, 0},
                    {0, 1}};*/
    //cout << "Size of nhood " << sizeof(nhood)/sizeof(nhood[0]) << endl;
    //cout << "rows: " << image.rows << ", cols: " << image.cols << endl;

    rgb.convertTo(imageFilt, CV_32F);
    // 0 - 255 range
    cv::cvtColor(imageFilt, imageFilt, cv::COLOR_RGB2GRAY);
//    double minVal, maxVal;
//    cv::minMaxIdx(imageFloat, &minVal, &maxVal);
//    cout << "minVal = " << minVal << endl;
//    cout << "maxVal = " << maxVal << endl;
    //resize(imageFloat, imageFloat, Size(320, 240));
    GaussianBlur(imageFilt, imageFilt, cv::Size(0, 0), sigma);
    GaussianBlur(depthFilt, depthFilt, cv::Size(0, 0), sigma);
//    split(imageFloat, imageChannels);

    int nrows = imageFilt.rows;
    int ncols = imageFilt.cols;

    cv::Mat segments(nrows, ncols, CV_32SC1, cv::Scalar(-1));

    vector<SegEdge> edges;
    for(int r = 0; r < nrows; r++){
        for(int c = 0; c < ncols; c++){
            for(int nh = 0; nh < sizeof(nhood)/sizeof(nhood[0]); nh++){
                int nhr = r + nhood[nh][0];
                int nhc = c + nhood[nh][1];
                if((nhr < nrows) && (nhr >= 0) &&
                   (nhc < ncols) && (nhc >= 0) &&
                    mask.at<uint8_t>(r, c) > 0 &&
                    mask.at<uint8_t>(nhr, nhc) > 0)
                {
//                    float diffAll = 0;
//                    for(int ch = 0; ch < nchannels; ch++){
//                        float diff = abs(imageChannels[ch].at<float>(r, c) - imageChannels[ch].at<float>(r + nhood[nh][0], c + nhood[nh][1]));
//                        diffAll += diff*diff;
//                    }
//                    diffAll = sqrt(diffAll);
                    float diffRgb = imageFilt.at<float>(r, c) - imageFilt.at<float>(nhr, nhc);
                    float diffDepth = (depthFilt.at<float>(r, c) - depthFilt.at<float>(nhr, nhc))/depthFilt.at<float>(r, c) ;

                    edges.push_back(SegEdge(c + ncols*r, nhc + ncols*nhr, 0.5*abs(diffRgb) + 0.5*64*abs(diffDepth)));
                    //if(edges.back().i == 567768 || edges.back().j == 567768){
                    //	cout << "diff = abs(" << (int)imageChannels[ch].at<unsigned char>(r, c) << " - " << (int)imageChannels[ch].at<unsigned char>(r + nhood[nh][0], c + nhood[nh][1]) << ") = " << diff << endl;
                    //}
                }
            }
        }
    }
    sort(edges.begin(), edges.end());

    endSorting = high_resolution_clock::now();
    cout << "End sorting" << endl;

    //cout << "Channel " << ch << endl;

    //cout << "Largest differece = " << edges[edges.size() - 1].weight <<
    //		", between (" << edges[edges.size() - 1].i << ", " << edges[edges.size() - 1].j <<
    //		")" << endl;

    UnionFind sets(nrows * ncols);
    vector<float> intDiff;
    intDiff.assign(nrows * ncols, 0);
    for(vector<SegEdge>::iterator it = edges.begin(); it != edges.end(); it++){
        int uRoot = sets.findSet(it->u);
        int vRoot = sets.findSet(it->v);
        //cout << "i = " << it->i << ", j = " << it->j << ", weight = " << it->weight << endl;
        if(uRoot != vRoot){
            //cout << "intDiff[uRoot] + (float)k/sizes[uRoot] = " << intDiff[uRoot] << " + " << (float)k/sizes[uRoot] << " = " << intDiff[uRoot] + (float)k/sizes[uRoot] << endl;
            //cout << "intDiff[vRoot] + (float)k/sizes[vRoot] = " << intDiff[vRoot] << " + " << (float)k/sizes[vRoot] << " = " << intDiff[vRoot] + (float)k/sizes[vRoot] << endl;
            if(min(intDiff[uRoot] + (float)k/sets.size(uRoot), intDiff[vRoot] + (float)k/sets.size(vRoot))
               >=
               it->w)
            {
                //cout << "union " << min(intDiff[uRoot] + (float)k/sizes[uRoot], intDiff[vRoot] + (float)k/sizes[vRoot]) << " >= " << it->weight << endl;
                int newRoot = sets.unionSets(uRoot, vRoot);
                intDiff[newRoot] = it->w;
            }
        }
    }
    cout << "Mergining small segments" << endl;
    for(vector<SegEdge>::iterator it = edges.begin(); it != edges.end(); it++){
        int iRoot = sets.findSet(it->u);
        int jRoot = sets.findSet(it->v);
        if((iRoot != jRoot) && ((sets.size(iRoot) < minSegment) || (sets.size(jRoot) < minSegment))){
            sets.unionSets(iRoot, jRoot);
        }
    }

    cout << "Counting elements" << endl;
//    set<int> numElements;
    map<int, int> idToIdx;
    int idxCnt = 0;
    for(int r = 0; r < nrows; r++){
        for(int c = 0; c < ncols; c++){
            if(mask.at<uint8_t>(r, c) > 0) {
                int id = sets.findSet(c + ncols * r);
                if (idToIdx.count(id) == 0) {
                    idToIdx[id] = idxCnt++;
                }
                int idx = idToIdx[id];

                segments.at<int>(r, c) = idx;
//            numElements.insert(sets.findSet(c + ncols*r));
            }
        }
    }
    cout << "number of elements = " << idToIdx.size() << endl;

    endComp = high_resolution_clock::now();

    endMerging = high_resolution_clock::now();

    static duration<double> sortingTime = duration<double>::zero();
    static duration<double> compTime = duration<double>::zero();
    static duration<double> mergingTime = duration<double>::zero();
    static duration<double> wholeTime = duration<double>::zero();
    static int times = 0;

    sortingTime += duration_cast<duration<double> >(endSorting - start);
    compTime += duration_cast<duration<double> >(endComp - endSorting);
    mergingTime += duration_cast<duration<double> >(endMerging - endComp);
    wholeTime += duration_cast<duration<double> >(endMerging - start);
    times++;
    cout << "Segment Times: " << times << endl;
    cout << "Segment Average sorting time: " << sortingTime.count()/times << endl;
    cout << "Segment Average computing time: " << compTime.count()/times << endl;
    cout << "Segment Average merging time: " << mergingTime.count()/times << endl;
    cout << "Segment Average whole time: " << wholeTime.count()/times << endl;

    return segments;
}

cv::Mat PlaneSegmentation::segmentRgb2(cv::Mat rgb, cv::Mat depth) {
    return cv::Mat();
}

void PlaneSegmentation::mergeSegments(vectorPlaneSeg &segs,
                                      UnionFind &sets,
                                      double curvThresh,
                                      double normalThresh,
                                      double stepThresh,
                                      pcl::visualization::PCLVisualizer::Ptr viewer,
                                      int viewPort1,
                                      int viewPort2)
{
    double shadingLevel = 16.0/256;

    vector<bool> visited(segs.size(), false);
    map<pair<int, int>, int> edgeVer;
//    vector<pair<int, int>> edges;
    priority_queue<SegEdge> bestEdges;
    for(int ps = 0; ps < segs.size(); ++ps){
        int curSeg = sets.findSet(ps);

        if(!visited[curSeg]) {
            for (int n = 0; n < segs[ps].getAdjSegs().size(); ++n) {
                int nh = sets.findSet(segs[ps].getAdjSegs()[n]);

                int u = std::min(curSeg, nh);
                int v = std::max(curSeg, nh);

                if(edgeVer.count(make_pair(u, v)) == 0){
                    edgeVer[make_pair(u, v)] = 1;

                    double score = compEdgeScore(segs[curSeg],
                                                 segs[nh],
                                                 curvThresh,
                                                 normalThresh,
                                                 stepThresh);

//                    if(u == 86 && v == 89){
//                        cout << "segs[curSeg].getSegCurv() = " << segs[curSeg].getSegCurv() << endl;
//                        cout << "segs[nh].getSegCurv() = " << segs[nh].getSegCurv() << endl;
//                        cout << "score = " << score << endl;
//                    }
                    SegEdge curEdge(u, v, 1, score);

                    bestEdges.push(curEdge);
                }
            }

            visited[curSeg] = true;
        }
    }

    vector<pair<int, int>> toMerge;
    while(!bestEdges.empty()){
        const SegEdge &curEdge = bestEdges.top();
        bestEdges.pop();

        int u = sets.findSet(curEdge.u);
        int v = sets.findSet(curEdge.v);

        pair<int, int> ep = make_pair(min(u, v), max(u, v));
        int newestVer = edgeVer[ep];

        if(viewer){
            drawSegments(viewer,
                         "cloud_lab",
                         viewPort1,
                         segs,
                         sets);

            viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY,
                                                     shadingLevel,
                                                     "cloud_lab",
                                                     viewPort1);

            viewer->removePointCloud("cloud_1", viewPort1);
            viewer->addPointCloud(segs[u].getPoints(), "cloud_1", viewPort1);
            viewer->removePointCloud("cloud_2", viewPort1);
            viewer->addPointCloud(segs[v].getPoints(), "cloud_2", viewPort1);

        }
        cout << "edge (" << u << ", " << v << ")" << endl;
        cout << "curEdge.w = " << curEdge.w << endl;

        cout << "newestVer = " << newestVer << endl;
        cout << "curEdge.ver = " << curEdge.ver << endl;

        bool newestEdge = false;

        // if the newest version
        if(u != v && newestVer == curEdge.ver) {
            newestEdge = true;
            if (checkIfCoplanar(segs[u], segs[v], curvThresh, normalThresh, stepThresh)) {

                // update versions
                map<pair<int, int>, int> prevEdgeVer = edgeVer;
                int highestVer = 0;
                vector<pair<int, int>> nhEdges;

                for (int n = 0; n < segs[u].getAdjSegs().size(); ++n) {
                    int nh = sets.findSet(segs[u].getAdjSegs()[n]);

                    nhEdges.emplace_back(min(u, nh), max(u, nh));
                }
                for (int n = 0; n < segs[v].getAdjSegs().size(); ++n) {
                    int nh = sets.findSet(segs[v].getAdjSegs()[n]);

                    nhEdges.emplace_back(min(v, nh), max(v, nh));
                }

                for(int ep = 0; ep < nhEdges.size(); ++ep){
                    highestVer = max(highestVer, edgeVer[nhEdges[ep]]);
                }
//                for(int ep = 0; ep < nhEdges.size(); ++ep){
//                    edgeVer[nhEdges[ep]] = highestVer + 1;
//                }

                // unite sets
                int m = sets.unionSets(u, v);
                cout << "Merging segments" << endl;
                segs[m] = segs[u].merge(segs[v], sets);
                cout << "end merging segments" << endl;

                // add new edges to heap
                for (int n = 0; n < segs[m].getAdjSegs().size(); ++n) {
                    int nh = sets.findSet(segs[m].getAdjSegs()[n]);

                    pair<int, int> ep = make_pair(min(m, nh), max(m, nh));
                    // still the same version -> adding edge
                    bool addEdge = false;
                    if(edgeVer.count(ep) == 0){
                        addEdge = true;
                    }
                    else if(prevEdgeVer[ep] >= edgeVer[ep]){
                        addEdge = true;
                    }

                    if(addEdge){
                        double score = compEdgeScore(segs[m],
                                                     segs[nh],
                                                     curvThresh,
                                                     normalThresh,
                                                     stepThresh);
                        int ver = highestVer + 1;
                        bestEdges.push(SegEdge(ep.first, ep.second, ver, score));

                        edgeVer[ep] = ver;
                    }
                }
            }
        }

        if(viewer && newestEdge){
            viewer->resetStoppedFlag();
            viewer->spinOnce (100);
            while (!viewer->wasStopped()){
                viewer->spinOnce (100);
                std::this_thread::sleep_for(std::chrono::milliseconds(50));
            }
        }
    }

}

void PlaneSegmentation::mergeSegmentsFF(vectorPlaneSeg &segs,
                                        UnionFind &sets,
                                        double curvThresh,
                                        double normalThresh,
                                        double stepThresh,
                                        pcl::visualization::PCLVisualizer::Ptr viewer,
                                        int viewPort1,
                                        int viewPort2)
{
    cout << "flood fill through segments" << endl;

    vector<bool> isVisited(segs.size(), false);
//    vector<vector<int> > planeCandidates;
    for(int sv = 0; sv < segs.size(); ++sv){
        // if not visited and planar enough
        if(!isVisited[sv] && segs[sv].getSegCurv() < curvThresh){
            queue<int> nodeQ;
            nodeQ.push(sv);
            isVisited[sv] = true;

            vector<int> curVisited;
            float curVisitedAreaCoeff = 0.0;
            while(!nodeQ.empty()){
                int curIdx = nodeQ.front();
                nodeQ.pop();

//                cout << "curIdx = " << curIdx << endl;

                curVisited.push_back(curIdx);
                curVisitedAreaCoeff += segs[curIdx].getAreaEst();
                for(int nh = 0; nh < segs[curIdx].getAdjSegs().size(); ++nh){
                    int nhIdx = segs[curIdx].getAdjSegs()[nh];
//                    cout << "nhIdx = " << nhIdx << endl;
                    // if not visited
                    if(!isVisited[nhIdx])
                    {
                        // if planar enough
                        if(segs[nhIdx].getSegCurv() < 2*curvThresh) {
                            Eigen::Vector3d centrVec = segs[nhIdx].getSegCentroid() -
                                                       segs[curIdx].getSegCentroid();
                            float step1 = std::fabs(centrVec.dot(segs[nhIdx].getSegNormal()));
                            float step2 = std::fabs((-centrVec).dot(segs[curIdx].getSegNormal()));
                            if(step1 < stepThresh && step2 < stepThresh){
                                float normalScore = segs[curIdx].getSegNormal().dot(segs[nhIdx].getSegNormal());

                                if(normalScore > normalThresh) {
                                    Eigen::Vector3d centrDir = centrVec.normalized();
                                    // variance of points scatter in a direction
                                    // of the line that connects centroids
                                    float varCentrDir1 = centrDir.transpose() * segs[curIdx].getSegCovar()
                                                         * centrDir;
                                    float varCentrDir2 = centrDir.transpose() * segs[nhIdx].getSegCovar()
                                                         * centrDir;
//                                    cout << "(" << curIdx << ", " << nhIdx << ")" << endl;
//                                    cout << "stddevCentrDir1 = " << sqrt(varCentrDir1) << endl;
//                                    cout << "stddevCentrDir2 = " << sqrt(varCentrDir2) << endl;
//                                    cout << "centrVec.norm() = " << centrVec.norm() << endl;

                                    nodeQ.push(nhIdx);
                                    isVisited[nhIdx] = true;
                                }
                            }

                        }
                    }
                }
            }

//            cout << "curVisited.size() = " << curVisited.size() << endl;
            for(int s = 1; s < curVisited.size(); ++s){
                int curSeg = sets.findSet(curVisited[s]);
                int prevSeg = sets.findSet(curVisited[s - 1]);
//                cout << "merging " << curSeg << " and " << prevSeg << endl;
//                cout << "segs[" << curSeg << "].getPoints()->size() = " << segs[curSeg].getPoints()->size() << endl;
//                cout << "segs[" << prevSeg << "].getPoints()->size() = " << segs[prevSeg].getPoints()->size() << endl;
                int m = sets.unionSets(curSeg, prevSeg);
                segs[m] = segs[curSeg].merge(segs[prevSeg], sets);
//                cout << "merged and inserted at " << m << endl;
//                cout << "segs[" << m << "].getPoints()->size() = " << segs[m].getPoints()->size() << endl;
            }
        }
    }
}

bool PlaneSegmentation::checkIfCoplanar(const PlaneSeg &seg1,
                                        const PlaneSeg &seg2,
                                        double curvThresh,
                                        double normalThresh,
                                        double stepThresh)
{
    bool coplanar = false;
    cout << "seg1.getSegCurv() = " << seg1.getSegCurv() << endl;
    cout << "seg2.getSegCurv() = " << seg2.getSegCurv() << endl;
    // if planar enough
    if((seg1.getSegCurv() < curvThresh && seg2.getSegCurv() < 2*curvThresh) ||
       (seg1.getSegCurv() < 2*curvThresh && seg2.getSegCurv() < curvThresh))
    {
        Eigen::Vector3d centrVec = seg2.getSegCentroid() -
                                   seg1.getSegCentroid();
        float step1 = std::fabs(centrVec.dot(seg2.getSegNormal()));
        float step2 = std::fabs((-centrVec).dot(seg1.getSegNormal()));
        cout << "step1 = " << step1 << endl;
        cout << "step2 = " << step2 << endl;
        // if there is no step between segments
        if(step1 < stepThresh && step2 < stepThresh){
            float normalScore = seg1.getSegNormal().dot(seg2.getSegNormal());
            cout << "normalScore = " << normalScore << endl;

            if(normalScore > normalThresh) {
                coplanar = true;
            }
        }

    }
    return coplanar;
}

double
PlaneSegmentation::compEdgeScore(const PlaneSeg &seg1,
                                const PlaneSeg &seg2,
                                double curvThresh,
                                double normalThresh,
                                double stepThresh) {
    double score = 0.0;

    double curvScore = std::min(1.0, std::exp(-(std::max(seg1.getSegCurv(), seg2.getSegCurv()) - curvThresh)/curvThresh));

    Eigen::Vector3d centrVec = seg2.getSegCentroid() -
                               seg1.getSegCentroid();
    double step1 = std::fabs(centrVec.dot(seg2.getSegNormal()));
    double step2 = std::fabs((-centrVec).dot(seg1.getSegNormal()));
    double stepScore = std::min(1.0, std::exp(-(std::max(step1, step2) - stepThresh)/stepThresh));

    double normalScore = seg1.getSegNormal().dot(seg2.getSegNormal());

    score = normalScore * stepScore * curvScore;

    return score;
}

void PlaneSegmentation::drawSegments(pcl::visualization::PCLVisualizer::Ptr viewer,
                                     std::string name,
                                     int vp,
                                     const vector<PlaneSeg, Eigen::aligned_allocator<PlaneSeg>> &segs,
                                     UnionFind &sets)
{
    viewer->removePointCloud(name, vp);

    pcl::PointCloud<pcl::PointXYZL>::Ptr pcLab(new pcl::PointCloud<pcl::PointXYZL>());

    int nextLab = 0;
    for(int s = 0; s < segs.size(); ++s){
        int set = sets.findSet(s);

        if(s == set) {
            int curLab = nextLab++;
            pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr points = segs[s].getPoints();

            for (int p = 0; p < points->size(); ++p) {
                pcl::PointXYZRGB pcPt = points->at(p);
                pcl::PointXYZL pt;
                pt.x = pcPt.x;
                pt.y = pcPt.y;
                pt.z = pcPt.z;
                pt.label = curLab;
                pcLab->push_back(pt);
            }
        }
    }

    viewer->addPointCloud(pcLab, name, vp);
}

void PlaneSegmentation::visualizeSegmentation(const vectorPlaneSeg &svs,
                                              const vectorPlaneSeg &segs,
                                              std::vector<int> &svLabels,
                                              pcl::visualization::PCLVisualizer::Ptr viewer,
                                              int viewPort1,
                                              int viewPort2)
{
    pcl::PointCloud<pcl::PointXYZL>::Ptr pcLabSegm(new pcl::PointCloud<pcl::PointXYZL>());

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pcVox(new pcl::PointCloud<pcl::PointXYZRGB>());
    pcl::PointCloud<pcl::Normal>::Ptr pcNormalsVox(new pcl::PointCloud<pcl::Normal>());
    for(int sv = 0; sv < svLabels.size(); ++sv) {
        const PlaneSeg& curSv = svs[sv];
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr points = svs[sv].getPoints();
        for(int p = 0; p < points->size(); ++p){
            pcl::PointXYZL pt;
            pt.x = points->at(p).x;
            pt.y = points->at(p).y;
            pt.z = points->at(p).z;
            pt.label = sv;

            pcLabSegm->push_back(pt);
        }

        pcVox->insert(pcVox->end(),
                      points->begin(),
                      points->end());
        pcNormalsVox->insert(pcNormalsVox->end(),
                             svs[sv].getNormals()->begin(),
                             svs[sv].getNormals()->end());
    }

    viewer->addPointCloud(pcLabSegm, "cloudLabSegm", viewPort1);
//        viewer->addPointCloud(pcVox, "cloud_color", viewPort1);
//        viewer->addPointCloudNormals<pcl::PointXYZRGBNormal>(pcNormals, 100, 0.1, "cloudNormals", viewPort1);
    viewer->addPointCloudNormals<pcl::PointXYZRGB, pcl::Normal>(pcVox, pcNormalsVox, 100, 0.1, "cloudNormals", viewPort1);

    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr pcColPlanes(new pcl::PointCloud<pcl::PointXYZRGBA>());
    pcl::PointCloud<pcl::PointNormal>::Ptr pcNormalsSv(new pcl::PointCloud<pcl::PointNormal>());

    for(int sv = 0; sv < svLabels.size(); ++sv) {
        // label -1 for non plane elements
        if (svLabels[sv] >= 0) {
            int lab = svLabels[sv];
            int colIdx = lab % (sizeof(colors)/sizeof(colors[0]));

            pcl::PointCloud<pcl::PointXYZRGB>::Ptr curPts = svs[sv].getPoints();
            for(int p = 0; p < curPts->size(); ++p){
                pcl::PointXYZRGBA pt;
                pt.r = colors[colIdx][0];
                pt.g = colors[colIdx][1];
                pt.b = colors[colIdx][2];
                pt.x = curPts->at(p).x;
                pt.y = curPts->at(p).y;
                pt.z = curPts->at(p).z;
                pcColPlanes->push_back(pt);
            }

            pcl::PointNormal curPtNormal = svs[sv].getPointNormal();
            pcNormalsSv->push_back(curPtNormal);
        }
    }

    viewer->addPointCloud(pcColPlanes, "cloudColPlanes", viewPort2);
//    viewer->addPointCloudNormals<pcl::PointNormal>(pcNormalsSv, 1, 0.1, "cloudNormalsSv", viewPort2);

//        cout << "pcNormals->size() = " << pcNormalsSv->size() << endl;
//		cout << "pc->size() = " << pc->size() << endl;
//		cout << "pcLab->size() = " << pcLab->size() << endl;
//
    viewer->resetStoppedFlag();
    viewer->initCameraParameters();
    viewer->setCameraPosition(0.0, 0.0, -4.0, 0.0, -1.0, 0.0);
    viewer->spinOnce (100);
    while (!viewer->wasStopped()){
        viewer->spinOnce (50);
        cv::waitKey(50);
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }
}

float PlaneSegmentation::compSvToPlaneDist(pcl::PointNormal sv,
									pcl::PointNormal plane)
{
	Eigen::Vector3f svNorm = sv.getNormalVector3fMap();
	Eigen::Vector3f planeNorm = plane.getNormalVector3fMap();

	float dist = distToPlane(plane, sv);

	float angDist = exp((1.0 - svNorm.dot(planeNorm))/0.1);
	if(isnan(angDist)){
		angDist = 1;
	}
	angDist = min(angDist, 10.0f);

	float weightedDist = angDist * dist;
	return weightedDist;
}

void PlaneSegmentation::compPlaneNormals(const std::vector<int>& svLabels,
                                    const vectorPlaneSeg& svsInfo,
									pcl::PointCloud<pcl::PointNormal>::Ptr planeNorm,
									std::vector<std::vector<int>>& planeSvsIdx)
{
	multimap<int, int> labelToSv;
	for(int sv = 0; sv < svLabels.size(); ++sv){
		labelToSv.insert(make_pair(svLabels[sv], sv));
	}

//	pcl::PointCloud<pcl::PointNormal>::Ptr newPlaneNorm(new pcl::PointCloud<pcl::PointNormal>());
	auto it = labelToSv.begin();
	while(it != labelToSv.end()){
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr curPc(new pcl::PointCloud<pcl::PointXYZRGB>());
		vector<int> curSvsIdx;

		auto itBegin = it;
		while(it != labelToSv.end() &&
			it->first == itBegin->first)
		{
			int curIdx = it->second;
//			int curId = idxToId[curIdx];
//			if(idToSv.count(curId) == 0){
//				throw PLANE_EXCEPTION("Id not fount in map, curId = " + to_string(curId) + ", curIdx = " + to_string(curIdx) + ", svLabels.size() = " + to_string(svLabels.size()));
//			}
			pcl::PointCloud<pcl::PointXYZRGB>::Ptr curSvPc(svsInfo[curIdx].getPoints());
			curPc->insert(curPc->begin(), curSvPc->begin(), curSvPc->end());
			curSvsIdx.push_back(curIdx);
			++it;
		}

		// if label != 0 than we have a plane
		if(itBegin->first != 0){
			pcl::PointNormal curPlaneNorm;
			Eigen::Vector4f curPlaneParams;
			float curCurv;
			pcl::computePointNormal(*curPc, curPlaneParams, curCurv);

			// Same work as in pcl::computePointNormal - could be done better
	//		EIGEN_ALIGN16 Eigen::Matrix3f covMat;
			Eigen::Vector4f centr;
			pcl::compute3DCentroid(*curPc, centr);

			Eigen::Vector3f curNormal = curPlaneParams.head<3>();
			curNormal.normalize();
//			pcl::flipNormalTowardsViewpoint(curPlaneNorm, 0.0, 0.0, 0.0, curNormal);

			curPlaneNorm.getVector4fMap() = centr;
			curPlaneNorm.getNormalVector3fMap() = curNormal;
//			curPlaneNorm.data_n[3] = 0.0;
//			curPlaneNorm.getNormalVector3fMap().normalize();
			curPlaneNorm.curvature = curCurv;

			planeNorm->push_back(curPlaneNorm);
			planeSvsIdx.push_back(curSvsIdx);
		}
	}
}

void PlaneSegmentation::compSupervoxelsAreaEst(const std::map<uint32_t, pcl::Supervoxel<pcl::PointXYZRGB>::Ptr >& idToSv,
										std::vector<float>& svAreaEst)
{
	svAreaEst.clear();
	for(auto it = idToSv.begin(); it != idToSv.end(); ++it){
		float curAreaCoeff = 0.0;
		pcl::Supervoxel<pcl::PointXYZRGB>::Ptr curSv = it->second;
//		pcl::Normal curNorm = curSv->normal_;
		pcl::PointNormal curNormPt;
		curSv->getCentroidPointNormal(curNormPt);
		Eigen::Vector3f curNorm = curNormPt.getNormalVector3fMap();
		for(int pt = 0; pt < curSv->voxels_->size(); ++pt){
			pcl::PointXYZRGB curPt = curSv->voxels_->at(pt);
			Eigen::Vector3f curPtCentroid = curPt.getVector3fMap() - curNormPt.getVector3fMap();
			float lenSq = curPtCentroid.squaredNorm();
			float normLen = curNorm.dot(curPtCentroid);
			float distPlaneSq = lenSq - normLen * normLen;
			if(!isnan(distPlaneSq)){
				curAreaCoeff = max(distPlaneSq, curAreaCoeff);
			}
		}
		svAreaEst.push_back(curAreaCoeff * pi);
	}
//	cout << "svAreaEst = " << svAreaEst << endl;
}

bool operator<(const SegEdge &l, const SegEdge &r){
    return l.w < r.w;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2014.
// Modifications copyright (c) 2014 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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)

#include <cstddef>
#include <iterator>
#include <string>

#include <algorithms/test_convex_hull.hpp>

#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

#include <boost/geometry/io/wkt/read.hpp>

#include <boost/geometry/multi/core/point_order.hpp>
#include <boost/geometry/multi/core/point_type.hpp>

#include <boost/geometry/multi/views/detail/range_type.hpp>

#include <boost/geometry/multi/algorithms/num_points.hpp>
#include <boost/geometry/multi/algorithms/detail/for_each_range.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/multi/io/wkt/wkt.hpp>

#include <boost/geometry/multi/geometries/multi_point.hpp>
#include <boost/geometry/multi/geometries/multi_linestring.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

template <typename P>
void test_all()
{
    typedef bg::model::multi_point<P> mp;
    typedef bg::model::multi_linestring<bg::model::linestring<P> > ml;
    typedef bg::model::multi_polygon<bg::model::polygon<P> > mpoly;

    // All points below in upper-points and lower-points
    test_geometry<mp>("MULTIPOINT((0 0),(5 0),(1 1),(4 1))", -1, 5, 4.0);
    test_geometry<mp>("MULTIPOINT((0 1),(5 1),(1 0),(4 0))", -1, 5, 4.0);

    // All points in vertical line (this delivers an empty polygon with 3 points and closing point for closed)
    test_geometry<mp>("MULTIPOINT((1 0),(5 0),(3 0),(4 0),(2 0))", -1, 4, 0.0);

    // One point only
    test_geometry<mp>("MULTIPOINT((1 0))", -1, 4, 0.0);

    // Problem of 6019, reproduced by the convex hull robustness test:
    test_geometry<mp>("MULTIPOINT((2 9),(1 3),(9 4),(1 1),(1 0),(7 9),(2 5),(3 7),(3 6),(2 4))",
            -1, 6, 48.0);

    // Ticket 6019:
    test_geometry<mp>("MULTIPOINT((0 53),(0 103),(0 53),(0 3),(0 3),(0 0),(1 0),(1 1),(2 1),(2 0),(2 0),(2 0),(3 0),(3 1),(4 1),(4 0),(5 0),(0 3),(10 3),(10 2),(10 2),(10 2),(5 2),(5 0),(5 0),(55 0),(105 0))",
            -1, 4, 5407.5);
    // Ticket 6021:
    test_geometry<mp>("multipoint((0 53), (0 103), (1 53))", 3, 4, 25);

    test_geometry<mp>("multipoint((1.1 1.1), (2.5 2.1), (3.1 3.1), (4.9 1.1), (3.1 1.9))", 5, 4, 3.8);
    test_geometry<ml>("multilinestring((2 4, 3 4, 3 5), (4 3,4 4,5 4))", 6, 5, 3.0);
    test_geometry<mpoly>("multipolygon(((1 4,1 6,2 5,3 5,4 6,4 4,1 4)), ((4 2,4 3,6 3,6 2,4 2)))", 12, 7, 14.0);

    test_empty_input<mp>();
    test_empty_input<ml>();
    test_empty_input<mpoly>();
}

int test_main(int, char* [])
{
    //test_all<bg::model::d2::point_xy<int> >();
    //test_all<bg::model::d2::point_xy<float> >();
    test_all<bg::model::d2::point_xy<double> >();

#ifdef HAVE_TTMATH
    test_all<bg::model::d2::point_xy<ttmath_big> >();
#endif

    return 0;
}


/*!
 * @file
 * This file defines unit tests for `boost::mpl::is_adjacent`.
 */

#include <boost/mpl/is_adjacent.hpp>
#include <boost/mpl/vector.hpp>

using namespace boost::mpl;

template <typename ...AdjacentVertices>
struct vertex {
    using open_neighborhood = vector<AdjacentVertices...>;
};

struct dummy_graph;

namespace test_acyclic_graph {
    struct v0 : vertex<> { };
    struct v1 : vertex<v0> { };
    struct v2 : vertex<v0> { };
    struct v3 : vertex<v1, v2> { };

    static_assert(!is_adjacent<dummy_graph, v0, v0>::value, "");
    static_assert(!is_adjacent<dummy_graph, v0, v1>::value, "");
    static_assert(!is_adjacent<dummy_graph, v0, v2>::value, "");
    static_assert(!is_adjacent<dummy_graph, v0, v3>::value, "");

    static_assert(is_adjacent<dummy_graph, v1, v0>::value, "");
    static_assert(!is_adjacent<dummy_graph, v1, v1>::value, "");
    static_assert(!is_adjacent<dummy_graph, v1, v2>::value, "");
    static_assert(!is_adjacent<dummy_graph, v1, v3>::value, "");

    static_assert(is_adjacent<dummy_graph, v2, v0>::value, "");
    static_assert(!is_adjacent<dummy_graph, v2, v1>::value, "");
    static_assert(!is_adjacent<dummy_graph, v2, v2>::value, "");
    static_assert(!is_adjacent<dummy_graph, v2, v3>::value, "");

    static_assert(!is_adjacent<dummy_graph, v3, v0>::value, "");
    static_assert(is_adjacent<dummy_graph, v3, v1>::value, "");
    static_assert(is_adjacent<dummy_graph, v3, v2>::value, "");
    static_assert(!is_adjacent<dummy_graph, v3, v3>::value, "");
}

int main() { }


//  (C) Copyright Matt Borland 2021.
//  Use, modification and distribution are subject to 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)

#ifndef BOOST_MATH_CCMATH_COPYSIGN_HPP
#define BOOST_MATH_CCMATH_COPYSIGN_HPP

#include <cmath>
#include <cstdint>
#include <limits>
#include <type_traits>
#include <boost/math/tools/is_constant_evaluated.hpp>
#include <boost/math/ccmath/abs.hpp>

namespace boost::math::ccmath {

namespace detail {

template <typename T>
inline constexpr T copysign_impl(const T mag, const T sgn) noexcept
{
    if(sgn >= 0)
    {
        return boost::math::ccmath::abs(mag);
    }
    else
    {
        return -boost::math::ccmath::abs(mag);
    }
}

} // Namespace detail

template <typename Real, std::enable_if_t<!std::is_integral_v<Real>, bool> = true>
inline constexpr Real copysign(Real mag, Real sgn) noexcept
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(mag))
    {
        return boost::math::ccmath::detail::copysign_impl(mag, sgn);
    }
    else
    {
        using std::copysign;
        return copysign(mag, sgn);
    }
}

template <typename T1, typename T2>
inline constexpr auto copysign(T1 mag, T2 sgn) noexcept
{
    if(BOOST_MATH_IS_CONSTANT_EVALUATED(mag))
    {
        // If the type is an integer (e.g. epsilon == 0) then set the epsilon value to 1 so that type is at a minimum
        // cast to double
        constexpr auto T1p = std::numeric_limits<T1>::epsilon() > 0 ? std::numeric_limits<T1>::epsilon() : 1;
        constexpr auto T2p = std::numeric_limits<T2>::epsilon() > 0 ? std::numeric_limits<T2>::epsilon() : 1;

        using promoted_type =
                              #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
                              std::conditional_t<T1p <= LDBL_EPSILON && T1p <= T2p, T1,
                              std::conditional_t<T2p <= LDBL_EPSILON && T2p <= T1p, T2,
                              #endif
                              std::conditional_t<T1p <= DBL_EPSILON && T1p <= T2p, T1,
                              std::conditional_t<T2p <= DBL_EPSILON && T2p <= T1p, T2, double
                              #ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
                              >>>>;
                              #else
                              >>;
                              #endif

        return boost::math::ccmath::copysign(promoted_type(mag), promoted_type(sgn));
    }
    else
    {
        using std::copysign;
        return copysign(mag, sgn);
    }
}

inline constexpr float copysignf(float mag, float sgn) noexcept
{
    return boost::math::ccmath::copysign(mag, sgn);
}

#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
inline constexpr long double copysignl(long double mag, long double sgn) noexcept
{
    return boost::math::ccmath::copysign(mag, sgn);
}
#endif

} // Namespaces

#endif // BOOST_MATH_CCMATH_COPYSIGN_HPP


#ifndef STAN_MATH_PRIM_PROB_BETA_RNG_HPP
#define STAN_MATH_PRIM_PROB_BETA_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/log_sum_exp.hpp>
#include <stan/math/prim/fun/max_size.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a Beta random variate with the supplied success and failure
 * parameters using the given random number generator.
 *
 * alpha and beta can each be a scalar or a one-dimensional container. Any
 * non-scalar inputs must be the same size.
 *
 * @tparam T_shape1 type of success parameter
 * @tparam T_shape2 type of failure parameter
 * @tparam RNG type of random number generator
 * @param alpha (Sequence of) positive finite success parameter(s)
 * @param beta (Sequence of) positive finite failure parameter(s)
 * @param rng random number generator
 * @return (Sequence of) beta random variate(s)
 * @throw std::domain_error if alpha or beta are nonpositive
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_shape1, typename T_shape2, class RNG>
inline typename VectorBuilder<true, double, T_shape1, T_shape2>::type beta_rng(
    const T_shape1 &alpha, const T_shape2 &beta, RNG &rng) {
  using boost::variate_generator;
  using boost::random::gamma_distribution;
  using boost::random::uniform_real_distribution;
  static const char *function = "beta_rng";
  check_positive_finite(function, "First shape parameter", alpha);
  check_positive_finite(function, "Second shape parameter", beta);
  check_consistent_sizes(function, "First shape parameter", alpha,
                         "Second shape Parameter", beta);

  scalar_seq_view<T_shape1> alpha_vec(alpha);
  scalar_seq_view<T_shape2> beta_vec(beta);
  size_t N = max_size(alpha, beta);
  VectorBuilder<true, double, T_shape1, T_shape2> output(N);

  variate_generator<RNG &, uniform_real_distribution<>> uniform_rng(
      rng, uniform_real_distribution<>(0.0, 1.0));
  for (size_t n = 0; n < N; ++n) {
    // If alpha and beta are large, trust the usual ratio of gammas
    // method for generating beta random variables. If any parameter
    // is small, work in log space and use Marsaglia and Tsang's trick
    if (alpha_vec[n] > 1.0 && beta_vec[n] > 1.0) {
      variate_generator<RNG &, gamma_distribution<>> rng_gamma_alpha(
          rng, gamma_distribution<>(alpha_vec[n], 1.0));
      variate_generator<RNG &, gamma_distribution<>> rng_gamma_beta(
          rng, gamma_distribution<>(beta_vec[n], 1.0));
      double a = rng_gamma_alpha();
      double b = rng_gamma_beta();
      output[n] = a / (a + b);
    } else {
      variate_generator<RNG &, gamma_distribution<>> rng_gamma_alpha(
          rng, gamma_distribution<>(alpha_vec[n] + 1, 1.0));
      variate_generator<RNG &, gamma_distribution<>> rng_gamma_beta(
          rng, gamma_distribution<>(beta_vec[n] + 1, 1.0));
      double log_a = std::log(uniform_rng()) / alpha_vec[n]
                     + std::log(rng_gamma_alpha());
      double log_b
          = std::log(uniform_rng()) / beta_vec[n] + std::log(rng_gamma_beta());
      double log_sum = log_sum_exp(log_a, log_b);
      output[n] = std::exp(log_a - log_sum);
    }
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


// Copyright 2008-2022 Emil Dotchevski and Reverge Studios, Inc.

// 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)

#ifdef BOOST_QVM_TEST_SINGLE_HEADER
#   include BOOST_QVM_TEST_SINGLE_HEADER
#else
#   include <boost/qvm/deduce_scalar.hpp>
#   include <boost/qvm/mat.hpp>
#   include <boost/qvm/mat_operations.hpp>
#   include <boost/qvm/vec.hpp>
#   include <boost/qvm/vec_mat_operations3.hpp>
#endif

#include <boost/core/lightweight_test.hpp>

template <class T>
struct
wrap
    {
    T t;
    wrap() { }
    explicit wrap(T t):t(t) { }
    };

template <class S, class T>
wrap<T>
operator*(S s, wrap<T> w)
    {
    return wrap<T>(s * w.t);
    }

template <class T>
wrap<T> operator+(wrap<T> a, wrap<T> b)
    {
    return wrap<T>(a.t + b.t);
    }

namespace
boost
    {
    namespace
    qvm
        {
        template <class T>
        struct
        is_scalar<wrap<T> >
            {
            static bool const value=true;
            };
        template <class S, class T>
        struct
        deduce_scalar<S, wrap<T> >
            {
            typedef wrap<typename deduce_scalar<S, T>::type> type;
            };
        }
    }

int
main()
    {
    using namespace boost::qvm;
    mat<double, 3, 3> m = rotz_mat<3>(3.14159);
    vec<wrap<double>, 3> v;
    v.a[0] = wrap<double>(1.0);
    v.a[1] = wrap<double>(0);
    v.a[2] = wrap<double>(0);
    vec<wrap<double>, 3> r = m * v;
    BOOST_TEST_LT(fabs(r.a[0].t+1), 0.0001);
    BOOST_TEST_LT(fabs(r.a[1].t), 0.0001);
    BOOST_TEST_LT(fabs(r.a[2].t), 0.0001);
    return boost::report_errors();
    }


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/function/std.hpp>
#include <boost/simd/function/fms.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <utility>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( remquo_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_<bd::floating_<A0> >
                          , bd::scalar_<bd::floating_<A0> >
                          )
  {
    using result_type = std::pair<A0,int>;

    BOOST_FORCEINLINE result_type operator() ( A0 a0, A0 a1
                                             ) const BOOST_NOEXCEPT
    {
      int q;
      A0 r = std::remquo(a0, a1, &q);
      return {r, q};
    }
  };

   BOOST_DISPATCH_OVERLOAD ( remquo_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_<bd::floating_<A0> >
                          , bd::scalar_<bd::floating_<A0> >
                          )
  {
    using quo_t = bd::as_integer_t<A0, signed>;
    using result_type = std::pair<A0,quo_t>;

    BOOST_FORCEINLINE std::pair<A0, quo_t> operator() ( A0 a0, A0 a1
                                      ) const BOOST_NOEXCEPT
    {
      A0 const d = nearbyint(a0/a1);

#if defined(BOOST_SIMD_NO_INVALIDS)
      A0  a2 = is_eqz(a1) ? Nan<A0>() : fma(-d, a1, a0);
#else
      A0  a2 = (is_invalid(a0) || is_eqz(a1)) ? Nan<A0>() : fma(-d, a1, a0);
#endif
      return {a2, toint(d)};
    }
  };
} } }

#endif