/**
 * \copyright
 * Copyright (c) 2012-2016, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/LICENSE.txt
 */

#include <gtest/gtest.h>

#include <limits>
#include <algorithm>
#include <vector>

#include <Eigen/Eigen>

#include "NumLib/Fem/CoordinatesMapping/ShapeMatrices.h"
#include "NumLib/Fem/CoordinatesMapping/NaturalCoordinatesMapping.h"

#include "CoordinatesMappingTestData/TestLine2.h"
#include "CoordinatesMappingTestData/TestTri3.h"
#include "CoordinatesMappingTestData/TestQuad4.h"
#include "CoordinatesMappingTestData/TestHex8.h"

#include "../TestTools.h"

using namespace NumLib;
using namespace CoordinatesMappingTestData;

namespace
{

// Element types to be tested
typedef ::testing::Types<
        TestLine2,
        TestTri3,
        TestQuad4,
        TestHex8
        > TestTypes;
}

template <class T_TEST>
class NumLibFemNaturalCoordinatesMappingTest : public ::testing::Test, public T_TEST
{
public:
    typedef typename T_TEST::ElementType ElementType;
    typedef typename T_TEST::ShapeFunctionType ShapeFunctionType;
    static const unsigned dim = T_TEST::dim;
    static const unsigned e_nnodes = T_TEST::e_nnodes;
    static const unsigned global_dim = T_TEST::global_dim;
    // Matrix types
    typedef Eigen::Matrix<double, e_nnodes, 1> NodalVector;
    typedef Eigen::Matrix<double, dim, e_nnodes, Eigen::RowMajor> DimNodalMatrix;
    typedef Eigen::Matrix<double, dim, dim, Eigen::RowMajor> DimMatrix;
    typedef Eigen::Matrix<double, global_dim, e_nnodes, Eigen::RowMajor> GlobalDimNodalMatrix;
    // Shape data type
    typedef ShapeMatrices<NodalVector,DimNodalMatrix,DimMatrix,GlobalDimNodalMatrix> ShapeMatricesType;
    // Natural coordinates mapping type
    typedef NaturalCoordinatesMapping<ElementType, ShapeFunctionType, ShapeMatricesType> NaturalCoordsMappingType;

public:
    NumLibFemNaturalCoordinatesMappingTest()
    : eps(std::numeric_limits<double>::epsilon())
    {
        // create four elements used for testing
        naturalEle   = this->createNaturalShape();
        irregularEle = this->createIrregularShape();
        clockwiseEle  = this->createClockWise();
        zeroVolumeEle  = this->createZeroVolume();

        // for destructor
        vec_eles.push_back(naturalEle);
        vec_eles.push_back(irregularEle);
        vec_eles.push_back(clockwiseEle);
        vec_eles.push_back(zeroVolumeEle);
        for (auto e : vec_eles)
            for (unsigned i=0; i<e->getNumberOfNodes(); i++)
                vec_nodes.push_back(e->getNode(i));
    }

    virtual ~NumLibFemNaturalCoordinatesMappingTest()
    {
        for (auto itr = vec_nodes.begin(); itr!=vec_nodes.end(); ++itr )
            delete *itr;
        for (auto itr = vec_eles.begin(); itr!=vec_eles.end(); ++itr )
            delete *itr;
    }

    const double eps;
    std::vector<const MeshLib::Node*> vec_nodes;
    std::vector<const ElementType*> vec_eles;
    ElementType* naturalEle;
    ElementType* irregularEle;
    ElementType* clockwiseEle;
    ElementType* zeroVolumeEle;
};

TYPED_TEST_CASE(NumLibFemNaturalCoordinatesMappingTest, TestTypes);

TYPED_TEST(NumLibFemNaturalCoordinatesMappingTest, CheckFieldSpecification_N)
{
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;
    typedef typename TestFixture::NaturalCoordsMappingType NaturalCoordsMappingType;
    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);

    //only N
    NaturalCoordsMappingType::template computeShapeMatrices<ShapeMatrixType::N>(*this->naturalEle, this->r, shape);
    ASSERT_FALSE(shape.N.isZero());
    ASSERT_TRUE(shape.dNdr.isZero());
    ASSERT_TRUE(shape.J.isZero());
    ASSERT_TRUE(shape.detJ == .0);
    ASSERT_TRUE(shape.invJ.isZero());
    ASSERT_TRUE(shape.dNdx.isZero());
}

TYPED_TEST(NumLibFemNaturalCoordinatesMappingTest, CheckFieldSpecification_DNDR)
{
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;
    typedef typename TestFixture::NaturalCoordsMappingType NaturalCoordsMappingType;
    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);

    // dNdr
    NaturalCoordsMappingType::template computeShapeMatrices<ShapeMatrixType::DNDR>(*this->naturalEle, this->r, shape);
    ASSERT_TRUE(shape.N.isZero());
    ASSERT_FALSE(shape.dNdr.isZero());
    ASSERT_TRUE(shape.J.isZero());
    ASSERT_TRUE(shape.detJ == .0);
    ASSERT_TRUE(shape.invJ.isZero());
    ASSERT_TRUE(shape.dNdx.isZero());
}

TYPED_TEST(NumLibFemNaturalCoordinatesMappingTest, CheckFieldSpecification_N_J)
{
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;
    typedef typename TestFixture::NaturalCoordsMappingType NaturalCoordsMappingType;
    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);

    // N_J
    shape.setZero();
    NaturalCoordsMappingType::template computeShapeMatrices<ShapeMatrixType::N_J>(*this->naturalEle, this->r, shape);
    ASSERT_FALSE(shape.N.isZero());
    ASSERT_FALSE(shape.dNdr.isZero());
    ASSERT_FALSE(shape.J.isZero());
    ASSERT_FALSE(shape.detJ == .0);
    ASSERT_TRUE(shape.invJ.isZero());
    ASSERT_TRUE(shape.dNdx.isZero());
}

TYPED_TEST(NumLibFemNaturalCoordinatesMappingTest, CheckFieldSpecification_DNDR_J)
{
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;
    typedef typename TestFixture::NaturalCoordsMappingType NaturalCoordsMappingType;
    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);

    // dNdr, J
    NaturalCoordsMappingType::template computeShapeMatrices<ShapeMatrixType::DNDR_J>(*this->naturalEle, this->r, shape);
    ASSERT_TRUE(shape.N.isZero());
    ASSERT_FALSE(shape.dNdr.isZero());
    ASSERT_FALSE(shape.J.isZero());
    ASSERT_FALSE(shape.detJ == .0);
    ASSERT_TRUE(shape.invJ.isZero());
    ASSERT_TRUE(shape.dNdx.isZero());
}

TYPED_TEST(NumLibFemNaturalCoordinatesMappingTest, CheckFieldSpecification_DNDX)
{
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;
    typedef typename TestFixture::NaturalCoordsMappingType NaturalCoordsMappingType;
    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);

    // DNDX
    shape.setZero();
    NaturalCoordsMappingType::template computeShapeMatrices<ShapeMatrixType::DNDX>(*this->naturalEle, this->r, shape);
    ASSERT_TRUE(shape.N.isZero());
    ASSERT_FALSE(shape.dNdr.isZero());
    ASSERT_FALSE(shape.J.isZero());
    ASSERT_FALSE(shape.detJ == .0);
    ASSERT_FALSE(shape.invJ.isZero());
    ASSERT_FALSE(shape.dNdx.isZero());
}

TYPED_TEST(NumLibFemNaturalCoordinatesMappingTest, CheckFieldSpecification_ALL)
{
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;
    typedef typename TestFixture::NaturalCoordsMappingType NaturalCoordsMappingType;
    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);

    // ALL
    shape.setZero();
    NaturalCoordsMappingType::computeShapeMatrices(*this->naturalEle, this->r, shape);
    ASSERT_FALSE(shape.N.isZero());
    ASSERT_FALSE(shape.dNdr.isZero());
    ASSERT_FALSE(shape.J.isZero());
    ASSERT_FALSE(shape.detJ == .0);
    ASSERT_FALSE(shape.invJ.isZero());
    ASSERT_FALSE(shape.dNdx.isZero());
}

TYPED_TEST(NumLibFemNaturalCoordinatesMappingTest, CheckNaturalShape)
{
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;
    typedef typename TestFixture::NaturalCoordsMappingType NaturalCoordsMappingType;

    // identical to natural coordinates
    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);
    NaturalCoordsMappingType::computeShapeMatrices(*this->naturalEle, this->r, shape);
    double exp_J[TestFixture::dim*TestFixture::dim]= {0.0};
    for (unsigned i=0; i<this->dim; i++)
        exp_J[i+this->dim*i] = 1.0;

    ASSERT_ARRAY_NEAR(this->nat_exp_N, shape.N.data(), shape.N.size(), this->eps);
    ASSERT_ARRAY_NEAR(this->nat_exp_dNdr, shape.dNdr.data(), shape.dNdr.size(), this->eps);
    ASSERT_ARRAY_NEAR(exp_J, shape.J.data(), shape.J.size(), this->eps);
    ASSERT_ARRAY_NEAR(exp_J, shape.invJ.data(), shape.invJ.size(), this->eps);
    ASSERT_NEAR(1.0, shape.detJ, this->eps);
    ASSERT_ARRAY_NEAR(this->nat_exp_dNdr, shape.dNdx.data(), shape.dNdx.size(), this->eps);
}

TYPED_TEST(NumLibFemNaturalCoordinatesMappingTest, CheckIrregularShape)
{
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;
    typedef typename TestFixture::NaturalCoordsMappingType NaturalCoordsMappingType;

    // irregular shape
    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);
    NaturalCoordsMappingType::computeShapeMatrices(*this->irregularEle, this->r, shape);
    //std::cout <<  std::setprecision(16) << shape;

    ASSERT_ARRAY_NEAR(this->nat_exp_N, shape.N.data(), shape.N.size(), this->eps);
    ASSERT_ARRAY_NEAR(this->nat_exp_dNdr, shape.dNdr.data(), shape.dNdr.size(), this->eps);
    ASSERT_ARRAY_NEAR(this->ir_exp_J, shape.J.data(), shape.J.size(), this->eps);
    ASSERT_NEAR(this->ir_exp_detJ, shape.detJ, this->eps);
    ASSERT_ARRAY_NEAR(this->ir_exp_invJ, shape.invJ.data(), shape.invJ.size(), this->eps);
    ASSERT_ARRAY_NEAR(this->ir_exp_dNdx, shape.dNdx.data(), shape.dNdx.size(), this->eps);
}

TYPED_TEST(NumLibFemNaturalCoordinatesMappingTest, CheckClockwise)
{
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;
    typedef typename TestFixture::NaturalCoordsMappingType NaturalCoordsMappingType;

    // clockwise node ordering, which is invalid)
    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);
    NaturalCoordsMappingType::computeShapeMatrices(*this->clockwiseEle, this->r, shape);
    //std::cout <<  std::setprecision(16) << shape;
    // Inverse of the Jacobian matrix doesn't exist
    double exp_invJ[TestFixture::dim*TestFixture::dim]= {0.0};
    double exp_dNdx[TestFixture::dim*TestFixture::e_nnodes]= {0.0};

    ASSERT_ARRAY_NEAR(this->nat_exp_N, shape.N.data(), shape.N.size(), this->eps);
    ASSERT_ARRAY_NEAR(this->nat_exp_dNdr, shape.dNdr.data(), shape.dNdr.size(), this->eps);
    ASSERT_ARRAY_NEAR(this->cl_exp_J, shape.J.data(), shape.J.size(), this->eps);
    ASSERT_NEAR(this->cl_exp_detJ, shape.detJ, this->eps);
    ASSERT_ARRAY_NEAR(exp_invJ, shape.invJ.data(), shape.invJ.size(), this->eps);
    ASSERT_ARRAY_NEAR(exp_dNdx, shape.dNdx.data(), shape.dNdx.size(), this->eps);
}

TYPED_TEST(NumLibFemNaturalCoordinatesMappingTest, CheckZeroVolume)
{
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;
    typedef typename TestFixture::NaturalCoordsMappingType NaturalCoordsMappingType;

    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);
    NaturalCoordsMappingType::computeShapeMatrices(*this->zeroVolumeEle, this->r, shape);
    //std::cout <<  std::setprecision(16) << shape;
    // Inverse of the Jacobian matrix doesn't exist
    double exp_invJ[TestFixture::dim*TestFixture::dim]= {0.0};
    double exp_dNdx[TestFixture::dim*TestFixture::e_nnodes]= {0.0};

    ASSERT_ARRAY_NEAR(this->nat_exp_N, shape.N.data(), shape.N.size(), this->eps);
    ASSERT_ARRAY_NEAR(this->nat_exp_dNdr, shape.dNdr.data(), shape.dNdr.size(), this->eps);
    ASSERT_ARRAY_NEAR(this->ze_exp_J, shape.J.data(), shape.J.size(), this->eps);
    ASSERT_NEAR(0.0, shape.detJ, this->eps);
    ASSERT_ARRAY_NEAR(exp_invJ, shape.invJ.data(), shape.invJ.size(), this->eps);
    ASSERT_ARRAY_NEAR(exp_dNdx, shape.dNdx.data(), shape.dNdx.size(), this->eps);
}

TEST(NumLib, FemNaturalCoordinatesMappingLineY)
{
    typedef Eigen::Matrix<double, 2, 1> NodalVector;
    typedef Eigen::Matrix<double, 1, 2, Eigen::RowMajor> DimNodalMatrix;
    typedef Eigen::Matrix<double, 1, 1, Eigen::RowMajor> DimMatrix;
    typedef Eigen::Matrix<double, 2, 2, Eigen::RowMajor> GlobalDimNodalMatrix;
    // Shape data type
    typedef ShapeMatrices<NodalVector,DimNodalMatrix,DimMatrix,GlobalDimNodalMatrix> ShapeMatricesType;
    typedef NaturalCoordinatesMapping<MeshLib::Line, ShapeLine2, ShapeMatricesType> MappingType;
    double r[] = {0.5};
    auto line = TestLine2::createY();
    static const unsigned dim = 1;
    static const unsigned e_nnodes = 2;
    ShapeMatricesType shape(dim, 2, e_nnodes);
    MappingType::computeShapeMatrices(*line, r, shape);

    double exp_J[dim*dim]= {0.0};
    for (unsigned i=0; i<dim; i++)
        exp_J[i+dim*i] = 1.0;

    const double eps(std::numeric_limits<double>::epsilon());
    ASSERT_ARRAY_NEAR(TestLine2::nat_exp_N, shape.N.data(), shape.N.size(), eps);
    ASSERT_ARRAY_NEAR(TestLine2::nat_exp_dNdr, shape.dNdr.data(), shape.dNdr.size(), eps);
    ASSERT_ARRAY_NEAR(exp_J, shape.J.data(), shape.J.size(), eps);
    ASSERT_ARRAY_NEAR(exp_J, shape.invJ.data(), shape.invJ.size(), eps);
    ASSERT_NEAR(1.0, shape.detJ, eps);
    double exp_dNdx[2*e_nnodes] = {0, 0, -0.5, 0.5};
    ASSERT_ARRAY_NEAR(exp_dNdx, shape.dNdx.data(), shape.dNdx.size(), eps);

    for (auto n = 0u; n < line->getNumberOfNodes(); ++n)
        delete line->getNode(n);
    delete line;
}


/////////////////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2021 Andreas Milton Maniotis.
//
// Email: andreas.maniotis@gmail.com
//
// 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)
/////////////////////////////////////////////////////////////////////////////////////////////

#include "aml/conslist.hpp"
#include "aml/string.hpp"

#include <type_traits>

#include <boost/core/demangle.hpp>
#include <iostream>

namespace test::conslist
{
    template<int n>
    struct type {};

    template<typename...>
    class Template {};

    void test_empty_list()
    {
        using aml::conslist;

        using empty = conslist<>;
        static_assert(empty::size() == 0, "");

        using list_one = conslist<>::cons<void>;
        using list_two = conslist<>::rcons<void>;

        static_assert(std::is_same< list_one, conslist<void> >::value, "");
        static_assert(std::is_same< list_two, conslist<void> >::value, "");
        static_assert(std::is_same< conslist<>::template apply<Template>, Template<> >::value, "");

        static_assert(std::is_same< empty, empty::reverse >::value, "");

        static_assert(std::is_same<conslist<>::lfold_with<Template, void***>, void***>::value, "");
        static_assert(std::is_same<conslist<>::rfold_with<Template, void***>, void***>::value, "");

        static_assert(std::is_same<aml::lfold<conslist<>, aml::function<Template>, void***>, void***>::value, "");
        static_assert(std::is_same<aml::rfold<conslist<>, aml::function<Template>, void***>, void***>::value, "");
    }

    void test_non_empty_list()
    {
        using aml::conslist;

        using list_t = conslist< type<1>, type<2>, type<3> >;

        static_assert(list_t::size() == 3, "");

        static_assert(std::is_same<list_t::cons<type<0> >,
                                   conslist< type<0>, type<1>, type<2>, type<3> > >::value, "");

        static_assert(std::is_same<list_t::rcons<type<0> >,
                                   conslist< type<1>, type<2>, type<3>, type<0> > >::value, "");

        static_assert(std::is_same<list_t::head, type<1> >::value, "");

        static_assert(std::is_same<list_t::tail, conslist<type<2>, type<3> > >::value, "");

        static_assert(std::is_same<list_t::reverse, conslist<type<3>, type<2>, type<1> > >::value, "");

        static_assert(std::is_same<list_t::apply<Template>, Template<type<1>, type<2>, type<3> > >::value, "");

        static_assert(std::is_same
                      <
                          list_t::lfold_with<Template, void***>,
                          Template<void***, Template< type<1>, Template<type<2>, type<3> > > >
                      >::value, "");

        static_assert(std::is_same
                      <
                          list_t::rfold_with<Template, void***>,
                          Template< type<1>, Template< type<2>, Template< type<3>, void***> > >
                      >::value, "");

        static_assert(std::is_same
                      <
                          list_t::lfold_with<Template, void***>,
                          aml::lfold< list_t, aml::function<Template>, void*** >
                      >::value, "");

        static_assert(std::is_same
                      <
                          list_t::rfold_with<Template, void***>,
                          aml::rfold< list_t, aml::function<Template>, void*** >
                      >::value, "");

    }

    template<auto >
    struct idx;

    void test_join()
    {
        using list_0 = aml::conslist<>;
        using list_1 = aml::conslist<idx<1> >;
        using list_2 = aml::conslist<idx<2>, idx<3> >;
        using list_3 = aml::conslist<idx<4>, idx<5>, idx<6> >;

        using j_0 = aml::conslist<>::join<>;
        using j_1 = aml::conslist<>::join<list_0>;
        using j_2 = aml::conslist<>::join<list_0, list_0>;
        using j_3 = aml::conslist<>::join<list_0, list_1>;
        using j_4 = aml::conslist<>::join<list_1, list_2, list_3>;
        using j_5 = aml::conslist<>::join<list_0, list_1, list_2, list_3>;

        static_assert(std::is_same<j_0, aml::conslist<>>::value, "");
        static_assert(std::is_same<j_1, aml::conslist<>>::value, "");
        static_assert(std::is_same<j_2, aml::conslist<>>::value, "");
        static_assert(std::is_same<j_3, aml::conslist<idx<1> > >::value, "");
        static_assert(std::is_same<j_4, aml::conslist<idx<1>, idx<2>, idx<3>, idx<4>, idx<5>, idx<6> > >::value, "");
        static_assert(std::is_same<j_5, j_4>::value, "");

        static_assert(std::is_same< aml::join<list_0, list_1, list_2, list_3>, j_4 >::value, "");
    }

}

#include <iostream>
#include <string>

int main()
{
    void (*test_set[])() =
    {
        test::conslist::test_empty_list,
        test::conslist::test_non_empty_list,
        test::conslist::test_join

    };

    for ( auto test : test_set )
        test();

    std::cout << __FILE__ << ": " << sizeof(test_set)/sizeof(test_set[0])  << " tests passed." << std::endl;

}


#pragma once

#include <Eigen/Dense>

namespace FMath::detail
{
    // Definition for OpenMP reduction operation using Vector3's
#   pragma omp declare reduction(+ : Vector3 : omp_out = omp_out + omp_in)\
        initializer(omp_priv = Vector3::Zero())

    template<typename T, typename Container>
    T Field<T, Container>::sum() const
    {
        T ret;
        if constexpr (std::is_same_v<T, Vector3>)
            ret.setZero();
        else
            ret = 0;

#       pragma omp parallel for reduction(+ : ret)
        for (std::size_t i = 0; i < size(); ++i)
            ret += _container[i];
        return ret;
    }

    template<typename T, typename Container>
    T Field<T, Container>::mean() const
    {
        return sum() / static_cast<long double>(size());
    }

    template<typename T, typename Container>
    scalar Field<T, Container>::min() const
    {
        static_assert(
            std::is_arithmetic<T>(),
            "Field<...>.min() is only available on Field<scalar>");

        scalar minval = std::numeric_limits<scalar>::max();

#       pragma omp parallel for reduction(min : minval)
        for (std::size_t i = 0; i < size(); ++i)
            if (_container[i] < minval) minval = _container[i];

        return minval;
    }

    template<typename T, typename Container>
    scalar Field<T, Container>::max() const
    {
        static_assert(
            std::is_arithmetic<T>(),
            "Field<...>.max() is only available on Field<scalar>");

        scalar maxval = -std::numeric_limits<scalar>::max();

#       pragma omp parallel for reduction(max : maxval)
        for (std::size_t i = 0; i < size(); ++i)
        {
            if (_container[i] > maxval) maxval = _container[i];
        }

        return maxval;
    }

    template<typename T, typename Container>
    std::pair<scalar, scalar> Field<T, Container>::minmax() const
    {
        static_assert(
            std::is_arithmetic<T>(),
            "Field<...>.minmax() is only available on Field<scalar>");

        scalar minval = std::numeric_limits<scalar>::max();
        scalar maxval = -std::numeric_limits<scalar>::max();

#       pragma omp parallel for reduction(min : minval) reduction(max : maxval)
        for (std::size_t i = 0; i < size(); ++i)
        {
            if (_container[i] < minval) minval = _container[i];
            if (_container[i] > maxval) maxval = _container[i];
        }

        return { minval, maxval };
    }

    template<typename T, typename Container>
    scalar Field<T, Container>::min_component() const
    {
        static_assert(
            std::is_same_v<T, Vector3>,
            "Field<...>.min_component() is only available on Field<Vector3>");

        scalar minval = std::numeric_limits<scalar>::max();

#       pragma omp parallel for reduction(min : minval)
        for (std::size_t i = 0; i < size(); ++i)
            for (int dim = 0; dim < 3; ++dim)
                if (_container[i][dim] < minval) minval = _container[i][dim];

        return minval;
    }

    template<typename T, typename Container>
    scalar Field<T, Container>::max_component() const
    {
        static_assert(
            std::is_same_v<T, Vector3>,
            "Field<...>.max_component() is only available on Field<Vector3>");

        scalar maxval = -std::numeric_limits<scalar>::max();

#       pragma omp parallel for reduction(max : maxval)
        for (std::size_t i = 0; i < size(); ++i)
            for (int dim = 0; dim < 3; ++dim)
                if (_container[i][dim] > maxval) maxval = _container[i][dim];

        return maxval;
    }

    template<typename T, typename Container>
    std::pair<scalar, scalar> Field<T, Container>::minmax_component() const
    {
        static_assert(
            std::is_same_v<T, Vector3>,
            "Field<...>.minmax_component() is only available on Field<Vector3>");

        scalar minval = std::numeric_limits<scalar>::max();
        scalar maxval = -std::numeric_limits<scalar>::max();

#       pragma omp parallel for reduction(min : minval) reduction(max : maxval)
        for (std::size_t i = 0; i < size(); ++i)
        {
            for (int dim = 0; dim < 3; ++dim)
            {
                if (_container[i][dim] < minval) minval = _container[i][dim];
                if (_container[i][dim] > maxval) maxval = _container[i][dim];
            }
        }

        return { minval, maxval };
    }
}


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

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

// 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)
//
// Example combining Boost.Geometry with Boost.Assign and Boost.Range and Boost.Tuple

#include <iostream>

#include <boost/geometry/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

#include <boost/assign.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian);

int main(void)
{
    using namespace boost::geometry;
    using namespace boost::assign;

    {
        typedef model::d2::point_xy<double> point;
        typedef model::polygon<point> polygon;
        typedef model::linestring<point> linestring;

        // Boost.Assign automatically works for linestring, rings, multi-geometries
        // It works because these are std:: containers

        // Using Boost.Assign operator +=
        linestring ls1;
        ls1 += point(1,2);
        ls1 += point(3,4), point(5,6), point(7,8);
        std::cout << dsv(ls1) << std::endl;

        // Using Boost.Assign operator()
        linestring ls2;
        push_back(ls2)(point(1, 2))(point(3, 4));
        std::cout << dsv(ls2) << std::endl;

        // Using Boost.Assign list_of
        linestring ls3 = list_of(point(1,2))(point(3,4));
        std::cout << dsv(ls3) << std::endl;

        // Using Boost.Assign + Boost.Range
        linestring ls4;
        push_back(ls4)(point(0, 0)).range(ls2).range(ls3);
        std::cout << dsv(ls4) << std::endl;

        // For a ring, it is similar to a linestring.
        // For a multi-point or multi-linestring, it is also similar
        // For a polygon, take the exterior ring or one of the interiors
        polygon p;
        push_back(exterior_ring(p))
            (point(0, 0))
            (point(0, 2))
            (point(2, 2))
            (point(2, 0))
            (point(0, 0))
            ;

        std::cout << dsv(p) << std::endl;
    }

    {
        // It is convenient to combine Boost.Assign on a geometry (e.g. polygon) with tuples.
        typedef model::polygon<boost::tuple<double,double> > polygon;

        polygon p;
        exterior_ring(p) = tuple_list_of(0, 0)(0, 5)(5, 5)(5, 0)(0, 0);

        std::cout << dsv(p) << std::endl;

        // And let it work on the interior_rings as well
        push_back(interior_rings(p))
            (tuple_list_of(1, 1)(2, 1)(2, 2)(1, 2)(1, 1))
            (tuple_list_of(3, 3)(4, 3)(4, 4)(3, 4)(3, 3))
            ;

        std::cout << "Area of " << dsv(p) << " is " << area(p) << std::endl;
    }

    return 0;
}


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

  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/constant/invlog_10.hpp>
#include <boost/simd/as.hpp>
#include <simd_test.hpp>

STF_CASE_TPL( "Check invlog_10 behavior for integral types"
            , (std::uint32_t)(std::uint64_t)
              (std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::invlog_10;
  using boost::simd::Invlog_10;
  T ref = T(0);
  STF_TYPE_IS(decltype(Invlog_10<T>()), T);
  STF_EQUAL(Invlog_10<T>(), ref);
  STF_EQUAL(invlog_10( as(T{}) ),ref);
}

STF_CASE_TPL( "Check invlog_10 behavior for float/double"
            , (float)(double)
            )
{
  using boost::simd::as;
  using boost::simd::detail::invlog_10;
  using boost::simd::Invlog_10;
  T ref = T(0.434294481903251827651128918917);
  STF_TYPE_IS(decltype(Invlog_10<T>()), T);
  STF_IEEE_EQUAL(Invlog_10<T>(), ref);
  STF_IEEE_EQUAL(invlog_10( as(T{}) ), ref);
}


#include <iostream>
#include <boost/thread/thread.hpp>
/* common_headers.h --> pcl_base.h --> Eigen/core.h */
#include <pcl/common/common_headers.h>
#include <pcl/range_image/range_image.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/range_image_visualizer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>

typedef pcl::PointXYZ PointType;
float angular_resolution = 0.5f;
pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::CAMERA_FRAME;
bool live_update = false;

void printUsage (const char* progName)
{
	std::cout << "\n\nUsage: "<<progName<<" [options] <scene.pcd>\n\n"
	    << "Options:\n"
	    << "-------------------------------------------\n"
	    << "-r <float>   angular resolution in degrees (default "<<angular_resolution<<")\n"
	    << "-c <int>     coordinate frame (default "<< (int)coordinate_frame<<")\n"
	    << "-l           live update - update the range image according to the selected view in the 3D viewer.\n"
	    << "-h           this help\n"
	    << "\n\n";
}

void setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose)
{
	Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(0, 0, 0);
	Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f(0, 0, 1) + pos_vector;
	Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f(0, -1, 0);
	/*
		void pcl::visualization::PCLVisualizer::setCameraParameters(const Camera &camera,
										int viewport = 0)
	*/
	/*
	viewer.camera_.pos[0] = pos_vector[0];
	viewer.camera_.pos[1] = pos_vector[1];
	viewer.camera_.pos[2] = pos_vector[2];
	viewer.camera_.focal[0] = look_at_vector[0];
	viewer.camera_.focal[1] = look_at_vector[1];
	viewer.camera_.focal[2] = look_at_vector[2];
	viewer.camera_.view[0] = up_vector[0];
	viewer.camera_.view[1] = up_vector[1];
	viewer.camera_.view[2] = up_vector[2];
	viewer.updateCamera();
	*/

	pcl::visualization::Camera camera;
	camera.pos[0] = pos_vector[0];
	camera.pos[1] = pos_vector[1];
	camera.pos[2] = pos_vector[2];
	camera.focal[0] = look_at_vector[0];
	camera.focal[1] = look_at_vector[1];
	camera.focal[2] = look_at_vector[2];
	camera.view[0] = up_vector[0];
	camera.view[1] = up_vector[1];
	camera.view[2] = up_vector[2];

	viewer.setCameraParameters(camera);
}

int main (int argc, char** argv)
{
	if (pcl::console::find_argument (argc, argv, "-h") >= 0)
	{
		printUsage (argv[0]);
		return 0;
	}

	if (pcl::console::find_argument (argc, argv, "-l") >= 0)
	{
		live_update = true;
		std::cout << "Live update is on.\n";
	}

	if (pcl::console::parse (argc, argv, "-r", angular_resolution) >= 0)
		std::cout << "Setting angular resolution to "<<angular_resolution<<"deg.\n";

	int tmp_coordinate_frame;
	if (pcl::console::parse (argc, argv, "-c", tmp_coordinate_frame) >= 0)
	{
		coordinate_frame = pcl::RangeImage::CoordinateFrame (tmp_coordinate_frame);
		std::cout << "Using coordinate frame "<< (int)coordinate_frame<<".\n";
	}

	angular_resolution = pcl::deg2rad (angular_resolution);
	pcl::PointCloud<PointType>::Ptr point_cloud_ptr (new pcl::PointCloud<PointType>);
	pcl::PointCloud<PointType>& point_cloud = *point_cloud_ptr;
	Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());
	std::vector<int> pcd_filename_indices = pcl::console::parse_file_extension_argument (argc, argv, "pcd");

	if (!pcd_filename_indices.empty ())
	{
		std::string filename = argv[pcd_filename_indices[0]];
		if (pcl::io::loadPCDFile (filename, point_cloud) == -1)
		{
			std::cout << "Was not able to open file \""<<filename<<"\".\n";
			printUsage (argv[0]);
			return 0;
		}
		scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (point_cloud.sensor_origin_[0],
		                                             point_cloud.sensor_origin_[1],
		                                             point_cloud.sensor_origin_[2])) *
		        	    Eigen::Affine3f (point_cloud.sensor_orientation_);
	}
	else
	{
		std::cout << "\nNo *.pcd file given => Genarating example point cloud.\n\n";
		for (float x=-0.5f; x<=0.5f; x+=0.01f)
		{
			for (float y=-0.5f; y<=0.5f; y+=0.01f)
			{
				PointType point;  point.x = x;  point.y = y;  point.z = 2.0f - y;
				point_cloud.points.push_back (point);
			}
		}
		point_cloud.width = (int) point_cloud.points.size ();  point_cloud.height = 1;
	}

	float noise_level = 0.0;
	float min_range = 0.0f;
	int border_size = 1;
	boost::shared_ptr<pcl::RangeImage> range_image_ptr(new pcl::RangeImage);
	pcl::RangeImage& range_image = *range_image_ptr;
	range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
		scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);

	pcl::visualization::PCLVisualizer viewer ("3D Viewer");
	viewer.setBackgroundColor (1, 1, 1);
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
	viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
	viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
	//viewer.addCoordinateSystem (1.0f);
	//PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
	//viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
	viewer.initCameraParameters ();
	// setViewerPose(viewer, range_image.getTransformationToWorldSystem ());

	pcl::visualization::RangeImageVisualizer range_image_widget ("Range image");
	range_image_widget.showRangeImage (range_image);

	while (!viewer.wasStopped ())
	{
		range_image_widget.spinOnce ();
		viewer.spinOnce ();
		pcl_sleep (0.01);
		if (live_update)
		{
			scene_sensor_pose = viewer.getViewerPose();
			range_image.createFromPointCloud (point_cloud, angular_resolution, pcl::deg2rad (360.0f), pcl::deg2rad (180.0f),
				scene_sensor_pose, pcl::RangeImage::LASER_FRAME, noise_level, min_range, border_size);
			range_image_widget.showRangeImage (range_image);
		}
	}
}


// Copyright Louis Dionne 2013-2017
// 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/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/find_if.hpp>
#include <boost/hana/map.hpp>
#include <boost/hana/optional.hpp>

#include <laws/base.hpp>
#include <support/minimal_product.hpp>
namespace hana = boost::hana;

template <int i>
auto key() { return hana::test::ct_eq<i>{}; }

template <int i>
auto val() { return hana::test::ct_eq<-i>{}; }

template <int i, int j>
auto p() { return ::minimal_product(key<i>(), val<j>()); }

int main() {
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::find_if(hana::make_map(), hana::equal.to(key<1>())),
        hana::nothing
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::find_if(hana::make_map(p<1, 1>()), hana::equal.to(key<1>())),
        hana::just(val<1>())
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::find_if(hana::make_map(p<1, 1>()), hana::equal.to(key<2>())),
        hana::nothing
    ));

    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::find_if(hana::make_map(p<1, 1>(), p<2, 2>()), hana::equal.to(key<1>())),
        hana::just(val<1>())
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::find_if(hana::make_map(p<1, 1>(), p<2, 2>()), hana::equal.to(key<2>())),
        hana::just(val<2>())
    ));
    BOOST_HANA_CONSTANT_CHECK(hana::equal(
        hana::find_if(hana::make_map(p<1, 1>(), p<2, 2>()), hana::equal.to(key<3>())),
        hana::nothing
    ));
}


///////////////////////////////////////////////////////////////////////////////
// kernel::functional::meta_nw_visitor_unary.hpp                             //
//                                                                           //
//  Copyright 2009 Erwann Rogard. 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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_KERNEL_ESTIMATION_META_NW_VISITOR_UNARY_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_KERNEL_ESTIMATION_META_NW_VISITOR_UNARY_HPP_ER_2009
#include <boost/statistics/detail/kernel/estimation/detail/mean_accumulator.hpp>
#include <boost/statistics/detail/kernel/estimation/detail/nw_visitor_unary.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace kernel{

// meta_visitor_unary<F1,F2>::apply is a valid template argument to estimator
// See detail/nw_visitor_unary
template<
    typename F1,    // x extractor
    typename F2
>
struct meta_nw_visitor_unary
{

     template<
        typename K,
        typename X,
        typename A = typename statistics::detail::kernel::detail::mean_accumulator<
            typename K::result_type
        >::type
    >
     struct apply{
       typedef detail::nw_visitor_unary<K,X,F1,F2,A> type;
    };
};

}// kernel
}// detail
}// statistics
}// boost

#endif


/* pcmsolver_copyright_start */
/*
 *     PCMSolver, an API for the Polarizable Continuum Model
 *     Copyright (C) 2013-2016 Roberto Di Remigio, Luca Frediani and contributors
 *
 *     This file is part of PCMSolver.
 *
 *     PCMSolver is free software: you can redistribute it and/or modify
 *     it under the terms of the GNU Lesser General Public License as published by
 *     the Free Software Foundation, either version 3 of the License, or
 *     (at your option) any later version.
 *
 *     PCMSolver is distributed in the hope that it will be useful,
 *     but WITHOUT ANY WARRANTY; without even the implied warranty of
 *     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *     GNU Lesser General Public License for more details.
 *
 *     You should have received a copy of the GNU Lesser General Public License
 *     along with PCMSolver.  If not, see <http://www.gnu.org/licenses/>.
 *
 *     For information on the complete list of contributors to the
 *     PCMSolver API, see: <http://pcmsolver.readthedocs.io/>
 */
/* pcmsolver_copyright_end */

#include "catch.hpp"

#include <cmath>
#include <iostream>
#include <iomanip>
#include <limits>

#include <Eigen/Core>

#include "green/DerivativeTypes.hpp"
#include "bi_operators/IntegratorTypes.hpp"
#include "cavity/Element.hpp"
#include "cavity/GePolCavity.hpp"
#include "bi_operators/PurisimaIntegrator.hpp"
#include "green/SphericalDiffuse.hpp"
#include "TestingMolecules.hpp"
#include "green/UniformDielectric.hpp"
#include "green/Vacuum.hpp"
#include "utils/MathUtils.hpp"

SCENARIO("A collocation integrator with diagonal elements according to Purisima for D", "[bi_operators][bi_operators_purisima]")
{
    GIVEN("A GePol cavity for a single sphere in the origin")
    {
        double radius = 1.44;
        Molecule molec = dummy<0>(1.44 / bohrToAngstrom());
        double area = 10.0;
        GePolCavity cavity = GePolCavity(molec, area, 0.0, 100.0);
        Eigen::MatrixXd results = Eigen::MatrixXd::Zero(cavity.size(), cavity.size());
        Eigen::MatrixXd reference = Eigen::MatrixXd::Zero(cavity.size(), cavity.size());

        /*! \class PurisimaIntegrator
         *  \test \b PurisimaIntegratorTest_vacuum tests the evaluation by collocation of the vacuum matrix representations of S and D
         */
        WHEN("the vacuum Green's function is used")
        {
            Vacuum<AD_directional, PurisimaIntegrator> gf;
            THEN("the matrix elements of S are")
            {
                results = gf.singleLayer(cavity.elements());
                reference = cnpy::custom::npy_load<double>("vacuum_S_collocation.npy");
                for (int i = 0; i < cavity.size(); ++i) {
                    for (int j = 0; j < cavity.size(); ++j) {
                        REQUIRE(reference(i, j) == Approx(results(i, j)));
                    }
                }
            }
            AND_THEN("the matrix elements of D are")
            {
                results = gf.doubleLayer(cavity.elements());
                reference = cnpy::custom::npy_load<double>("vacuum_D_purisima.npy");
                for (int i = 0; i < cavity.size(); ++i) {
                    for (int j = 0; j < cavity.size(); ++j) {
                        REQUIRE(reference(i, j) == Approx(results(i, j)));
                    }
                }
            }
        }

        /*! \class PurisimaIntegrator
         *  \test \b PurisimaIntegratorTest_uniformdielectric tests the evaluation by collocation of the uniform dielectric matrix representations of S and D
         */
        AND_WHEN("the uniform dielectric Green's function is used")
        {
            double epsilon = 80.0;
            UniformDielectric<AD_directional, PurisimaIntegrator> gf(epsilon);
            THEN("the matrix elements of S are")
            {
                results = gf.singleLayer(cavity.elements());
                reference = cnpy::custom::npy_load<double>("uniformdielectric_S_collocation.npy");
                for (int i = 0; i < cavity.size(); ++i) {
                    for (int j = 0; j < cavity.size(); ++j) {
                        REQUIRE(reference(i, j) == Approx(results(i, j)));
                    }
                }
            }
            AND_THEN("the matrix elements of D are")
            {
                results = gf.doubleLayer(cavity.elements());
                reference = cnpy::custom::npy_load<double>("uniformdielectric_D_purisima.npy");
                for (int i = 0; i < cavity.size(); ++i) {
                    for (int j = 0; j < cavity.size(); ++j) {
                        REQUIRE(reference(i, j) == Approx(results(i, j)));
                    }
                }
            }
        }
    }
}


#include <algorithm>
#include <iostream>
#include <queue>
#include <vector>
#include <set>
#include <map>
#include <random>
#include <boost/pending/disjoint_sets.hpp>

namespace sublevel {
typedef std::vector<std::vector<int>> Graph;

struct Ans {
    Ans(int e, int d, int es, int ds)
        : eat(e), death(d), eat_size(es), death_size(ds),
        total_height(0.0) {
    }
    int eat;
    int death;
    int eat_size;
    int death_size;
    float total_height;
};

class RootedForest {
public:
    Graph tree_;
    std::vector<int> pred_;
    std::vector<int> height_;

    explicit RootedForest(Graph tree) : tree_(tree) {
    }

    void Initilize() {
        pred_.resize(tree_.size(), -1);
        height_.resize(tree_.size(), -1);
        std::queue<int> que;
        size_t counter = 0;
        while (counter < tree_.size()) {
            if (que.empty()) {
                int next_start = 0;
                while (height_[next_start] != -1) {
                    ++next_start;
                }
                que.push(next_start);
                ++counter;
                height_[next_start] = 0;
            }
            while (!que.empty()) {
                int current = que.front();
                que.pop();
                for (auto neu : tree_[current]) {
                    if (height_[neu] == -1) {
                        height_[neu] = height_[current] + 1;
                        pred_[neu] = current;
                        que.push(neu);
                        ++counter;
                    }
                }
            }
        }
    }

    bool GetWay (int lhs, int rhs, std::vector<int>& answer) {
        std::vector<int> left_way;
        std::vector<int> right_way;
        while (height_[lhs] > height_[rhs]) {
            left_way.push_back(lhs);
            lhs = pred_[lhs];
        }
        while (height_[lhs] < height_[rhs]) {
            right_way.push_back(rhs);
            rhs = pred_[rhs];
        }
        while ((lhs != -1 && rhs != -1) && lhs != rhs) {
            left_way.push_back(lhs);
            lhs = pred_[lhs];
            right_way.push_back(rhs);
            rhs = pred_[rhs];
        }
        if (lhs != -1 && rhs != -1) {
            for (size_t step = 0; step < left_way.size(); ++step) {
                answer.push_back(left_way[step]);
            }
            answer.push_back(lhs);
            for (size_t step = right_way.size(); step > 0; --step) {
                answer.push_back(right_way[step - 1]);
            }
            return true;
        }
        return false;
    }
};

class VirtualCloud {
public:
    float* values_;
    size_t cloud_size_;
    std::vector<int> order_;
    std::set<int> trash_;
    Graph graph_;
    Graph minima_graph_;
    VirtualCloud(float* val, size_t size) : values_(val), cloud_size_(size) {
    }
    virtual ~VirtualCloud() = default;
    void GetOrder() {
        order_.reserve(cloud_size_);
        for (int index = 0; index < static_cast<int>(cloud_size_); ++index) {
            order_.push_back(index);
        }
        std::sort(order_.begin(), order_.end(),
                  [&](int lhs, int rhs) { return (*(values_ + lhs) < *(values_ + rhs) ||
                                                  (*(values_ + lhs) == *(values_ + rhs) && lhs < rhs)); } );
    }

    virtual void GetGraph() {
    }

    virtual void SetGraph(Graph&& gr) {
        graph_ = gr;
    }

    std::map<int, Ans> SublevelHomology() {
        GetGraph();
        minima_graph_.resize(cloud_size_);
        std::map<int, Ans> answer;
        int lenght = graph_.size();
        std::vector<int> back_ord(lenght, 0);
        for (int id = 0; id < lenght; ++id) {
            back_ord[order_[id]] = id;
        }
        std::vector<int> siz(lenght + 1, 0);
        std::vector<int> ord(lenght + 1, 0);
        boost::disjoint_sets<int*,int*> ds(&ord[0], &siz[0]);
        auto comp = [&](int lhs, int rhs){ return back_ord[lhs - 1] < back_ord[rhs - 1]; };
        for (int ind = 0; ind < lenght; ++ind) {
            int vertex = order_[ind];
            if (trash_.find(vertex) != trash_.end()) {
                continue;
            }
            std::map<int, int, decltype(comp)> clusters(comp);
            for (auto neubour : graph_[vertex]) {
                if (trash_.find(neubour) == trash_.end()) {
                    int next_cluster = ds.find_set(neubour + 1);
                    if (clusters.find(next_cluster) == clusters.end()) {
                        clusters.emplace(next_cluster, neubour);
                    } else if (back_ord[neubour] < back_ord[clusters[next_cluster]]) {
                        clusters[next_cluster] = neubour;
                    }
                }
            }
            for (auto& item : clusters) {
                minima_graph_[vertex].push_back(item.second);
                minima_graph_[item.second].push_back(vertex);
            }
            if (clusters.empty()) {
                ds.make_set(vertex + 1);
                answer.emplace(vertex, Ans(-1, -1, -1, 1));
                continue;
            }
            int start = clusters.begin()->first;
            ds.make_set(vertex + 1);
            ds.link(vertex + 1, start);
            ++answer.at(start - 1).death_size;
            answer.at(start - 1).total_height += *(values_ + vertex) - *(values_ + start - 1);
            for (auto it = next(clusters.begin()); it != clusters.end(); ++it) {
                ord[start] = std::max(ord[start], ord[it->first]);
                ds.link(it->first, start);
                answer.at(it->first - 1).eat = start - 1;
                answer.at(it->first - 1).death = vertex;
                answer.at(it->first - 1).eat_size = answer.at(start - 1).death_size;
                answer.at(start - 1).death_size += answer.at(it->first - 1).death_size;
                answer.at(start - 1).total_height += answer.at(it->first - 1).total_height +
                                                     (*(values_ + it->first - 1) - *(values_ + start - 1)) *
                                                     answer.at(it->first - 1).death_size;
            }
        }
        return answer;
    }
};

class GridCloud : public VirtualCloud {
private:
    std::vector<size_t> shape_;
    std::vector<size_t> shift_;

    int GetIndex(int point, int dim) {
        return (point % shift_[dim + 1]) / shift_[dim];
    }

public:
    GridCloud(float* val, size_t size, std::vector<size_t> sh)
        : VirtualCloud(val, size), shape_(sh) {
        shift_.resize(shape_.size() + 1, 1);
        for (size_t id = 1; id < shape_.size() + 1; ++id) {
            shift_[id] = shift_[id - 1] * shape_[id - 1];
        }
    }

    void GetGraph() {
        graph_.resize(cloud_size_);
        for (size_t index = 0; index < cloud_size_; ++index) {
            for (size_t dim = 0; dim < shape_.size(); ++dim) {
                size_t down = index - shift_[dim];
                if (GetIndex(index, dim) != 0 && *(values_ + index) >= *(values_ + down)) {
                    graph_[index].push_back(down);
                }
                size_t up = index + shift_[dim];
                if (GetIndex(up, dim) != 0 && *(values_ + index) > *(values_ + up)) {
                    graph_[index].push_back(up);
                }
            }
        }
    }
};

}


/*
@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/ext/std/integer_sequence.hpp>

#include <boost/hana/comparable/laws.hpp>
#include <boost/hana/detail/assert.hpp>
#include <boost/hana/list/instance.hpp>

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

template <typename T, typename U>
void test() {
    BOOST_HANA_CONSTANT_ASSERT(
        Comparable::laws::check(
            list(
                std::integer_sequence<T>{},
                std::integer_sequence<U>{},

                std::integer_sequence<T, 0>{},
                std::integer_sequence<U, 0>{},

                std::integer_sequence<T, 0, 1>{},
                std::integer_sequence<U, 0, 1>{},

                std::integer_sequence<T, 1, 0, 2>{},
                std::integer_sequence<U, 1, 0, 2>{}
            )
        )
    );
}

int main() {
    test<int, int>();
    test<int, unsigned long long>();
}


// ---------------------------------------------------------------------
//
// Copyright (c) 2019 - 2021 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

#include <ibtk/config.h>

#include "InterpolationUtilities.h"
#include "tbox/MathUtilities.h"
#include "tbox/Pointer.h"

IBTK_DISABLE_EXTRA_WARNINGS
#include <Eigen/Core>
#include <Eigen/QR>
IBTK_ENABLE_EXTRA_WARNINGS

#include <HierarchyCellDataOpsReal.h>
#include <Patch.h>
#include <PatchLevel.h>

#include <algorithm>

using Eigen::Matrix3d;
using Eigen::Vector3d;

namespace IBTK
{
double
InterpolationUtilities::interpolate(const vector<double>& X,
                                    const int data_idx,
                                    Pointer<CellVariable<NDIM, double> > Q_var,
                                    Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
                                    const std::vector<RobinBcCoefStrategy<NDIM>*>& bc_coefs,
                                    const double data_time,
                                    const int depth)
{
    double q_val = 0.0;
    VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();
    const int data_idx_temp =
        var_db->registerVariableAndContext(Q_var, var_db->getContext("Interpolation"), IntVector<NDIM>(3));
    for (int ln = 0; ln <= patch_hierarchy->getFinestLevelNumber(); ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        level->allocatePatchData(data_idx_temp);
    }
    HierarchyCellDataOpsReal<NDIM, double> hier_cc_data_ops(patch_hierarchy);
    hier_cc_data_ops.copyData(data_idx_temp, data_idx);
    typedef HierarchyGhostCellInterpolation::InterpolationTransactionComponent InterpolationTransactionComponent;
    std::vector<InterpolationTransactionComponent> ghost_cell_components(1);
    ghost_cell_components[0] = InterpolationTransactionComponent(
        data_idx_temp, "CONSERVATIVE_LINEAR_REFINE", false, "CONSERVATIVE_COARSEN", "LINEAR", false, bc_coefs, NULL);
    HierarchyGhostCellInterpolation ghost_fill_op;
    ghost_fill_op.initializeOperatorState(ghost_cell_components, patch_hierarchy);
    ghost_fill_op.fillData(data_time);
    bool done = false;
    for (int ln = patch_hierarchy->getFinestLevelNumber(); ln >= 0 && !done; --ln)
    {
        // Start at the finest level...
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        CellIndex<NDIM> idx = IndexUtilities::getCellIndex(X, level->getGridGeometry(), level->getRatio());
        for (PatchLevel<NDIM>::Iterator p(level); p && !done; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            const Pointer<CartesianPatchGeometry<NDIM> > p_geom = patch->getPatchGeometry();
            const double* const dx = p_geom->getDx();
            const double* const x_lower = p_geom->getXLower();
            const Box<NDIM>& patch_box = patch->getBox();
            Pointer<CellData<NDIM, double> > S_data = patch->getPatchData(data_idx_temp);
            if (patch_box.contains(idx))
            {
                // Great. The patch is currently on this level
                // Let's create a box that contains this data
                Box<NDIM> box(idx, idx);
                // Grow it by some number of grid cells
                box.grow(IntVector<NDIM>(3));
                // Loop through the box, make sure the point is located
                // OUTSIDE the disk
                std::vector<double> x(NDIM);
                CellData<NDIM, int> i_data(box, 1, IntVector<NDIM>(0));
                CellData<NDIM, double> si_data(box, NDIM + 1, IntVector<NDIM>(0));
                si_data.fillAll(std::numeric_limits<double>::signaling_NaN());
                const CellIndex<NDIM> ci_l = patch_box.lower();
                int num = 0;
                for (CellIterator<NDIM> i(box); i; i++)
                {
                    CellIndex<NDIM> ci = i();
                    for (int d = 0; d < NDIM; ++d) x[d] = x_lower[d] + dx[d] * (ci(d) - ci_l(d) + 0.5);
                    double r = sqrt(x[0] * x[0] + x[1] * x[1]);
                    if (r > 1.0)
                    {
                        i_data(ci) = 1;
                        si_data(ci, NDIM) = (*S_data)(ci, depth);
                        num++;
                    }
                    else
                    {
                        i_data(ci) = -1;
                    }
                }
                // We have a box containing the point and data the says if we are inside or outside disk.
                // Find directions and interpolate. First in x, then in y.
                std::vector<int> completed_dims;
                q_val = InterpolationUtilities::interpolate_in_boxes(
                    idx, X, i_data, si_data, p_geom, patch_box, 0, 0, completed_dims);
                done = true;
            }
        }
    }
    for (int ln = 0; ln <= patch_hierarchy->getFinestLevelNumber(); ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        level->deallocatePatchData(data_idx_temp);
    }
    q_val = IBTK_MPI::sumReduction(q_val);
    return q_val;
}

double
InterpolationUtilities::interpolateL2(const std::vector<double>& X,
                                      const int data_idx,
                                      Pointer<CellVariable<NDIM, double> > Q_var,
                                      SAMRAI::tbox::Pointer<SAMRAI::hier::PatchHierarchy<NDIM> > patch_hierarchy,
                                      const std::vector<SAMRAI::solv::RobinBcCoefStrategy<NDIM>*>& bc_coefs,
                                      const double data_time,
                                      const int depth)
{
    double q_val = 0.0;
    VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();
    const int data_idx_temp =
        var_db->registerVariableAndContext(Q_var, var_db->getContext("Interpolation"), IntVector<NDIM>(3));
    for (int ln = 0; ln <= patch_hierarchy->getFinestLevelNumber(); ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        level->allocatePatchData(data_idx_temp);
    }
    HierarchyCellDataOpsReal<NDIM, double> hier_cc_data_ops(patch_hierarchy);
    hier_cc_data_ops.copyData(data_idx_temp, data_idx);
    typedef HierarchyGhostCellInterpolation::InterpolationTransactionComponent InterpolationTransactionComponent;
    std::vector<InterpolationTransactionComponent> ghost_cell_components(1);
    ghost_cell_components[0] = InterpolationTransactionComponent(
        data_idx_temp, "CONSERVATIVE_LINEAR_REFINE", false, "CONSERVATIVE_COARSEN", "LINEAR", false, bc_coefs, NULL);
    HierarchyGhostCellInterpolation ghost_fill_op;
    ghost_fill_op.initializeOperatorState(ghost_cell_components, patch_hierarchy);
    ghost_fill_op.fillData(data_time);
    bool done = false;
    for (int ln = patch_hierarchy->getFinestLevelNumber(); ln >= 0 && !done; --ln)
    {
        // Start at the finest level...
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        CellIndex<NDIM> idx = IndexUtilities::getCellIndex(X, level->getGridGeometry(), level->getRatio());
        for (PatchLevel<NDIM>::Iterator p(level); p && !done; p++)
        {
            Pointer<Patch<NDIM> > patch = level->getPatch(p());
            const Pointer<CartesianPatchGeometry<NDIM> > p_geom = patch->getPatchGeometry();
            const double* const dx = p_geom->getDx();
            const double* const x_lower = p_geom->getXLower();
            const Box<NDIM>& patch_box = patch->getBox();
            Pointer<CellData<NDIM, double> > S_data = patch->getPatchData(data_idx_temp);
            if (patch_box.contains(idx))
            {
                // Great. The patch is currently on this level
                // Let's create a box that contains this data
                Box<NDIM> box(idx, idx);
                // Grow it by some number of grid cells
                box.grow(2);
                // Loop through the box, make sure the point is located
                // OUTSIDE the disk
                std::vector<double> x(NDIM);
                CellData<NDIM, int> i_data(box, 1, IntVector<NDIM>(0));
                const CellIndex<NDIM> ci_l = patch_box.lower();
                int num = 0;
                for (CellIterator<NDIM> i(box); i; i++)
                {
                    CellIndex<NDIM> ci = i();
                    for (int d = 0; d < NDIM; ++d) x[d] = x_lower[d] + dx[d] * (ci(d) - ci_l(d) + 0.5);
                    double r = sqrt(x[0] * x[0] + x[1] * x[1]);
                    if (r > 1.0)
                    {
                        i_data(ci) = 1;
                        num++;
                    }
                }
                // Moving least squares
                VectorXd rhs = VectorXd::Zero(6);
                VectorXd soln = VectorXd::Zero(6);
                MatrixXd mat = MatrixXd::Zero(6, 6);
                for (CellIterator<NDIM> i(box); i; i++)
                {
                    CellIndex<NDIM> ci = i();
                    if (i_data(ci) == 1)
                    {
                        for (int d = 0; d < NDIM; ++d) x[d] = x_lower[d] + dx[d] * (ci(d) - ci_l(d) + 0.5);
                        // Fill in RHS and MATRIX values
                        double w = weight_fcn(X, x);
                        double f = (*S_data)(ci, depth);
                        rhs(0) += w * f;
                        rhs(1) += w * f * x[0];
                        rhs(2) += w * f * x[1];
                        rhs(3) += w * f * x[0] * x[0];
                        rhs(4) += w * f * x[1] * x[1];
                        rhs(5) += w * f * x[0] * x[1];
                        mat(0, 0) += w; /*mat(0,1) += x[0]*w; mat(0,2) += x[1]*w; mat(0,3) += x[0]*x[0]*w; mat(0,4) +=
                                           x[1]*x[1]*w; mat(0,5) += x[0]*x[1]*w;*/
                        mat(1, 0) += x[0] * w;
                        mat(1, 1) += x[0] * x[0] * w; /* mat(1,2) += x[0]*x[1]*w; mat(2,3) += x[0]*x[0]*x[0]*w; mat(2,4)
                                                         += x[0]*x[1]*x[1]*w; mat(3,5) += x[0]*x[0]*x[1]*w;*/
                        mat(2, 0) += x[1] * w;
                        mat(2, 1) += x[1] * x[0] * w;
                        mat(2, 2) += x[1] * x[1] * w;
                        mat(3, 0) += x[0] * x[0] * w;
                        mat(3, 1) += x[0] * x[0] * x[0] * w;
                        mat(3, 2) += x[0] * x[0] * x[1] * w;
                        mat(3, 3) += x[0] * x[0] * x[0] * x[0] * w;
                        mat(4, 0) += x[1] * x[1] * w;
                        mat(4, 1) += x[1] * x[1] * x[0] * w;
                        mat(4, 2) += x[1] * x[1] * x[1] * w;
                        mat(4, 3) += x[1] * x[1] * x[0] * x[0] * w;
                        mat(4, 4) += x[1] * x[1] * x[1] * x[1] * w;
                        mat(5, 0) += x[0] * x[1] * w;
                        mat(5, 1) += x[0] * x[1] * x[0] * w;
                        mat(5, 2) += x[0] * x[1] * x[1] * w;
                        mat(5, 3) += x[0] * x[1] * x[0] * x[0] * w;
                        mat(5, 4) += x[0] * x[1] * x[1] * x[1] * w;
                        mat(5, 5) += x[0] * x[1] * x[0] * x[1] * w;
                    }
                }
                //                mat(0,1) = mat(1,0); mat(0,2) = mat(2,0); mat(0,3) = mat(3,0); mat(0,4) = mat(4,0);
                //                mat(0,5) = mat(5,0); mat(1,2) = mat(2,1); mat(1,3) = mat(3,1); mat(1,4) = mat(4,1);
                //                mat(1,5) = mat(5,1); mat(2,3) = mat(3,2); mat(2,4) = mat(4,2); mat(2,5) = mat(5,2);
                //                mat(3,4) = mat(4,3); mat(3,5) = mat(5,3);
                //                mat(4,5) = mat(5,4);
                soln = mat.ldlt().solve(rhs);
                q_val = soln(0) + soln(1) * X[0] + soln(2) * X[1] + soln(3) * X[0] * X[0] + soln(4) * X[1] * X[1] +
                        soln(5) * X[0] * X[1];
                done = true;
            }
        }
    }
    q_val = IBTK_MPI::sumReduction(q_val);

    for (int ln = 0; ln <= patch_hierarchy->getFinestLevelNumber(); ++ln)
    {
        Pointer<PatchLevel<NDIM> > level = patch_hierarchy->getPatchLevel(ln);
        level->deallocatePatchData(data_idx_temp);
    }
    return q_val;
}

double
InterpolationUtilities::weight_fcn(const std::vector<double>& x, const std::vector<double>& x_j)
{
    double r = sqrt((x[0] - x_j[0]) * (x[0] - x_j[0]) + (x[1] - x_j[1]) * (x[1] - x_j[1]));
    double w = exp(-10.0 * r * r);
    w = 1.0;
    return w;
}

double
InterpolationUtilities::interpolate(const double& l, const std::vector<int>& i, const std::vector<double>& y)
{
    double val = 0.0;
    TBOX_ASSERT(i.size() == 3);
    TBOX_ASSERT(y.size() == i.size());
    val = y[0] + (y[1] - y[0]) * l / static_cast<double>(i[1] - i[0]) +
          (y[2] * static_cast<double>(i[0] - i[1]) + y[0] * static_cast<double>(i[1] - i[2]) +
           y[1] * static_cast<double>(i[2] - i[0])) *
              l * (l - 1.0) / (static_cast<double>((i[0] - i[1]) * (i[0] - i[2]) * (i[1] - i[2])));
    return val;
}

double
InterpolationUtilities::interpolate_in_boxes(const CellIndex<NDIM>& idx,
                                             const std::vector<double>& X,
                                             CellData<NDIM, int>& r_data,
                                             CellData<NDIM, double>& q_data,
                                             Pointer<CartesianPatchGeometry<NDIM> > pgeom,
                                             const Box<NDIM>& pbox,
                                             int dim,
                                             int cycle,
                                             std::vector<int>& completed_dims)
{
    double q_val = 0.0;
    // form list of indices.
    std::vector<CellIndex<NDIM> > idx_list;
    std::vector<int> i_list;
    bool done = false;
    while (!done)
    {
        idx_list.clear();
        i_list.clear();
        dim = dim % NDIM;
        if (r_data(idx) == 1)
        {
            idx_list.push_back(idx);
            i_list.push_back(0);
        }
        int s = 1;
        while (idx_list.size() < 3)
        {
            IntVector<NDIM> si(0);
            si(dim) = s;
            if (r_data(idx + si) == 1)
            {
                idx_list.push_back(idx + si);
                i_list.push_back(s);
            }
            if (r_data(idx - si) == 1)
            {
                idx_list.push_back(idx - si);
                i_list.push_back(-s);
            }
            s++;
        }
        int min_val = *std::min_element(i_list.begin(), i_list.end());
        int max_val = *std::max_element(i_list.begin(), i_list.end());
        if (std::max(std::abs(min_val), std::abs(max_val)) > 3)
        {
            dim++;
            if (std::find(completed_dims.begin(), completed_dims.end(), dim) != completed_dims.end() || dim > NDIM)
            {
                TBOX_ERROR("already completed dimension, or dimension too high");
            }
        }
        else
        {
            done = true;
        }
    }
    while (idx_list.size() > 3)
    {
        idx_list.pop_back();
    }
    completed_dims.push_back(dim);
    // We start at dim = 0. Each call generates a new box to interpolate inside of, and increases the dim by 1.
    if (cycle < NDIM - 1)
    {
        std::vector<int> i_list;
        std::vector<double> y_data;
        for (std::vector<CellIndex<NDIM> >::const_iterator cit = idx_list.begin(); cit != idx_list.end(); ++cit)
        {
            const CellIndex<NDIM>& cidx = *cit;
            q_data(cidx, cycle + 1) = InterpolationUtilities::interpolate_in_boxes(
                cidx, X, r_data, q_data, pgeom, pbox, dim + 1, cycle + 1, completed_dims);
            i_list.push_back(cidx(dim));
            y_data.push_back(q_data(cidx, cycle + 1));
        }
        const double* dx = pgeom->getDx();
        const double* xlow = pgeom->getXLower();
        const CellIndex<NDIM>& idxl = pbox.lower();
        double xx = xlow[dim] + dx[dim] * (idx_list[0](dim) - idxl(dim) + 0.5);
        q_val = InterpolationUtilities::interpolate((X[dim] - xx) / dx[dim], i_list, y_data);
    }
    else if (cycle == NDIM - 1)
    {
        std::vector<int> i_list;
        std::vector<double> y_data;
        for (std::vector<CellIndex<NDIM> >::const_iterator cit = idx_list.begin(); cit != idx_list.end(); ++cit)
        {
            const CellIndex<NDIM>& cidx = *cit;
            i_list.push_back(cidx(dim));
            y_data.push_back(q_data(cidx, cycle + 1));
        }
        const double* dx = pgeom->getDx();
        const double* xlow = pgeom->getXLower();
        const CellIndex<NDIM>& idxl = pbox.lower();
        double xx = xlow[dim] + dx[dim] * (idx_list[0](dim) - idxl(dim) + 0.5);
        q_val = InterpolationUtilities::interpolate((X[dim] - xx) / dx[dim], i_list, y_data);
    }
    else
    {
        // Shouldn't get here.
        TBOX_ERROR("Invalid dimension.\n");
    }
    return q_val;
}
} // namespace IBTK


#include "gate_est_node.hpp"

#include <geometry_msgs/TransformStamped.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_eigen/tf2_eigen.h>

#include <visualization_msgs/Marker.h>

#include <cstring>
#include <string>
#include <algorithm>
#include <memory>

#include <Eigen/Core>
#include <Eigen/Dense>

#include "depth_estimator.hpp"

#include "ba.hpp"

namespace gate_est
{

cv::Mat TransformToCvProjection(const geometry_msgs::Transform &transform)
{
  Eigen::Quaterniond quat(transform.rotation.w,
                          transform.rotation.x,
                          transform.rotation.y,
                          transform.rotation.z);
  Eigen::Matrix3d rot = quat.toRotationMatrix();

  cv::Mat proj = (cv::Mat_<double>(3, 4) << rot(0, 0), rot(0, 1), rot(0, 2), transform.translation.x,
                  rot(1, 0), rot(1, 1), rot(1, 2), transform.translation.y,
                  rot(2, 0), rot(2, 1), rot(2, 2), transform.translation.z);
  return proj;
}

cv::Point2d GateEst::Pixel2Camera(double x, double y)
{
  const double cx = k_.at<double>(0, 2);
  const double cy = k_.at<double>(1, 2);
  const double fx = k_.at<double>(0, 0);
  const double fy = k_.at<double>(1, 1);
  return cv::Point2d((x - cx) / fx, (y - cy) / fy);
}

void GateEst::BackProject(double x, double y, double depth, Eigen::Vector3d &out)
{
  const double cx = k_.at<double>(0, 2);
  const double cy = k_.at<double>(1, 2);
  const double fx = k_.at<double>(0, 0);
  const double fy = k_.at<double>(1, 1);
  out << (x - cx) * depth / fx,
      (y - cy) * depth / fy,
      depth;
}

void GateEst::Pixel2Camera(double x, double y, Eigen::Vector2d &out)
{
  const double cx = k_.at<double>(0, 2);
  const double cy = k_.at<double>(1, 2);
  const double fx = k_.at<double>(0, 0);
  const double fy = k_.at<double>(1, 1);
  out << (x - cx) / fx,
      (y - cy) / fy;
}

void GateEst::Pixel2Camera(double x, double y, cv::Point2d &out)
{
  const double cx = k_.at<double>(0, 2);
  const double cy = k_.at<double>(1, 2);
  const double fx = k_.at<double>(0, 0);
  const double fy = k_.at<double>(1, 1);
  out.x = (x - cx) / fx;
  out.y = (y - cy) / fy;
}

void GateEst::OdometryCallback(const nav_msgs::OdometryConstPtr& ptr) {
/*   const Eigen::MatrixXd& corners_cam = T_c_w.matrix() * gate_corners_est_;
  Eigen::MatrixXd corners_projected = cm_ * corners_cam.topRows(3);
  corners_projected.array().rowwise() /= corners_cam.row(2).array();

  ROS_INFO_STREAM(corners_projected.col(9 * 4 + 0));
  ROS_INFO_STREAM(corners_projected.col(9 * 4 + 1));
  ROS_INFO_STREAM(corners_projected.col(9 * 4 + 2));
  ROS_INFO_STREAM(corners_projected.col(9 * 4 + 3));
 */
  // 2. optimize gate corner position via bundle adjustments.

  geometry_msgs::TransformStamped transform;
  try {
    transform = buffer_.lookupTransform("camera", "map", ptr->header.stamp, ros::Duration(1.0/20));
  } catch (tf2::TransformException &ex) {
    ROS_WARN("Unable to get transform. Skipped.");
    return;
  }

  const Eigen::MatrixXd& T_c_w = tf2::transformToEigen(transform.transform).matrix();

  ceres::Problem problem;

  for (auto iter = p_ref_->landmarks_2d.begin(); iter != p_ref_->landmarks_2d.end(); ++iter)
  {
    std::size_t key = iter->first;
    std::size_t gate_idx = GetGateIndex(key) - 1;
    std::size_t corner_idx = GetCornerIndex(key) - 1;
    const Eigen::Vector2d& uv = iter->second;
    const Eigen::Vector3d& pt = gate_corners_[gate_idx][corner_idx];
    problem.AddResidualBlock(
      new ceres::AutoDiffCostFunction<CostFunctionReProjection, 2, 3>(
        new CostFunctionReProjection(pt, T_c_w, cm_, uv)),
        nullptr, &gate_offsets_[gate_idx][0]);
  }

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_QR;
  options.minimizer_progress_to_stdout = false;
  options.parameter_tolerance = 1e-5;

  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);

  visualization_msgs::Marker corners_marker;
  corners_marker.header.stamp = ros::Time::now();
  corners_marker.header.frame_id = "map";
  corners_marker.type = visualization_msgs::Marker::SPHERE_LIST;
  corners_marker.action = visualization_msgs::Marker::ADD;
  corners_marker.id = 1;
  corners_marker.color.r = 1.0;
  corners_marker.color.g = 1.0;
  corners_marker.color.a = 1.0;
  corners_marker.scale.x = 0.5;
  corners_marker.scale.y = 0.5;
  corners_marker.scale.z = 0.5;

  ROS_INFO_STREAM("Gate offsets:");
  for (std::size_t i = 0; i < NUM_GATES; ++i) {
    ROS_INFO_STREAM("Gate "<<i+1<<": "<<gate_offsets_[i][0]<<", "<<gate_offsets_[i][1]<<", "<<gate_offsets_[i][2]);
    for (int j = 0; j < 4; ++j) {
      geometry_msgs::Point pt;
      pt.x = gate_corners_[i][j][0] + gate_offsets_[i][0];
      pt.y = gate_corners_[i][j][1] + gate_offsets_[i][1];
      pt.z = gate_corners_[i][j][2] + gate_offsets_[i][2];
      corners_marker.points.push_back(pt);
      std_msgs::ColorRGBA c;
      c.r = 1.0;
      c.g = 1.0;
      c.a = 1.0;
      corners_marker.colors.push_back(c);

      geometry_msgs::Point pt_n;
      pt_n.x = gate_corners_[i][j][0];
      pt_n.y = gate_corners_[i][j][1];
      pt_n.z = gate_corners_[i][j][2];
      corners_marker.points.push_back(pt_n);
      std_msgs::ColorRGBA c_n;
      c_n.b = 1.0;
      c_n.a = 1.0;
      corners_marker.colors.push_back(c_n);
    }
  }
  corners_viz_pub_.publish(corners_marker);
}

void GateEst::IRMarkerArrayCallback(const flightgoggles::IRMarkerArrayConstPtr& ptr) {
  // store detected irmarker information for later use
  auto frame = Frame::MakePtr();
  for (auto iter = ptr->markers.begin(); iter != ptr->markers.end(); ++iter)
  {
    std::size_t gate_idx = std::atoi(iter->landmarkID.data.substr(4).c_str());
    std::size_t idx = std::atoi(iter->markerID.data.c_str());

    StoreLandmark(frame->landmarks_2d, gate_idx, idx, Eigen::Vector2d(iter->x, iter->y));
  }
  p_ref_ = frame;
}

void GateEst::Init()
{
  // read in course definition and initial pose, then establish a
  // reference frame base on these information
  LoadGateNominalInformation();
  LoadInitialPose();
  reference_gate_idx_ = 10;

  camera_info_sub_ = nh_.subscribe<sensor_msgs::CameraInfo>("/uav/camera/left/camera_info", 1, &GateEst::CameraInfoCallback, this);
  ir_beacons_sub_ = nh_.subscribe<flightgoggles::IRMarkerArray>("/uav/camera/left/ir_beacons", 1, &GateEst::IRMarkerArrayCallback, this);

  odometry_sub_ = nh_.subscribe<nav_msgs::Odometry>("/vins_estimator/odometry", 1, &GateEst::OdometryCallback, this);

  corners_viz_pub_ = nh_.advertise<visualization_msgs::Marker>("/GateEst/corners_viz", 1);
}

void GateEst::LoadGateNominalInformation()
{
  gate_corners_.clear();

  char buf[256];

  for (std::size_t i = 1; i <= NUM_GATES; ++i)
  {
    std::sprintf(buf, "/uav/Gate%zu/nominal_location", i);
    XmlRpc::XmlRpcValue corners;
    nh_.getParam(buf, corners);
    std::vector<Eigen::Vector3d> cs;
    std::vector<cv::Point3d> cs_rel;
    for (std::size_t j = 0; j < 4; ++j)
    {
      cs.push_back(Eigen::Vector3d(corners[j][0], corners[j][1], corners[j][2]));
      if (j == 0)
      {
        // 0, 0, 0 for the first corner
        cs_rel.push_back(cv::Point3d(0, 0, 0));
      }
      else
      {
        // position relative to the first corner for the others
        const Eigen::Vector3d v = cs[j] - cs[0];
        cs_rel.push_back(cv::Point3d(v[0], v[1], v[2]));
      }
    }

    gate_corners_.push_back(cs);
  }
  ROS_INFO("Nominal gates information loaded.");
}

void GateEst::LoadInitialPose()
{
  std::vector<double> pose;
  nh_.getParam("/uav/flightgoggles_uav_dynamics/init_pose", pose);
  initial_position_ << pose[0], pose[1], pose[2];
  initial_orientation_ = Eigen::Quaterniond(pose[6], pose[3], pose[4], pose[5]);
}

void GateEst::CameraInfoCallback(const sensor_msgs::CameraInfoConstPtr &ptr)
{
  k_ = (cv::Mat_<double>(3, 3)
            << ptr->K[0],
        ptr->K[1], ptr->K[2],
        ptr->K[3], ptr->K[4], ptr->K[5],
        ptr->K[6], ptr->K[7], ptr->K[8]);

  cm_ << ptr->K[0],ptr->K[1], ptr->K[2],
         ptr->K[3], ptr->K[4], ptr->K[5],
         ptr->K[6], ptr->K[7], ptr->K[8];
}

} // namespace gate_est

XROS_RUNNABLE_NODE_MAIN(gate_est::GateEst)