/*
 * Copyright (c) 2013, Christian Gehring, Hannes Sommer, Paul Furgale, Remo Diethelm
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL Christian Gehring, Hannes Sommer, Paul Furgale,
 * Remo Diethelm BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
*/

#include <iostream>

#include <Eigen/Core>

#include <gtest/gtest.h>

#include "kindr/poses/Pose.hpp"
#include "kindr/poses/PoseBase.hpp"
#include "kindr/phys_quant/PhysicalQuantities.hpp"

typedef ::testing::Types<
    kindr::HomTransformQuatD,
    kindr::HomTransformQuatF,
    kindr::HomTransformMatrixD,
    kindr::HomTransformMatrixF
> Types;

template <typename PoseImplementation>
struct HomogeneousTransformationTest: public ::testing::Test {
  typedef PoseImplementation Pose;
  typedef typename Pose::Scalar Scalar;
  typedef typename Pose::Position Position;
  typedef typename Pose::Rotation Rotation;
};

TYPED_TEST_CASE(HomogeneousTransformationTest, Types);

TYPED_TEST(HomogeneousTransformationTest, testCopyConstructor)
{
  typedef typename TestFixture::Pose Pose;
  typedef typename TestFixture::Position Position;
  typedef typename TestFixture::Rotation Rotation;
  typedef typename TestFixture::Scalar Scalar;

  Pose poseA(Position(1.0,2.0,3.0), Rotation(kindr::EulerAnglesZyx<Scalar>(0.5,1.2,-1.7)));
  Pose poseB(poseA);

  ASSERT_EQ(poseA.getPosition().x(), poseB.getPosition().x());
  ASSERT_EQ(poseA.getPosition().y(), poseB.getPosition().y());
  ASSERT_EQ(poseA.getPosition().z(), poseB.getPosition().z());
  ASSERT_TRUE(poseA.getRotation().isNear(poseB.getRotation(),1.0e-6));
}

TYPED_TEST(HomogeneousTransformationTest, testSetIdentity)
{
  typedef typename TestFixture::Pose Pose;
  typedef typename TestFixture::Position Position;
  typedef typename TestFixture::Rotation Rotation;
  typedef typename TestFixture::Scalar Scalar;
  Pose test(Position(1,2,3), Rotation(kindr::AngleAxis<Scalar>(0.5,1.0,0,0)));

  test.setIdentity();

  ASSERT_EQ(test.getPosition().vector().x(), 0.0);
  ASSERT_EQ(test.getPosition().vector().y(), 0.0);
  ASSERT_EQ(test.getPosition().vector().z(), 0.0);

  ASSERT_TRUE(test.getRotation().isNear(Rotation(),1.0e-6));
}

TYPED_TEST(HomogeneousTransformationTest, testComparisonEqual)
{
  typedef typename TestFixture::Pose Pose;
  typedef typename TestFixture::Position Position;
  typedef typename TestFixture::Rotation Rotation;
  typedef typename TestFixture::Scalar Scalar;
  Position positionAToBInA(1.0,2.0,3.0);
  Position positionAToCInA(1.0,2.0,4.0);
  Rotation rotationBToA(kindr::EulerAnglesZyx<Scalar>(0.5, -0.9, 1.2));
  Rotation rotationCToA(kindr::EulerAnglesZyx<Scalar>(0.5, -0.9, 1.2));
  Pose poseBToA(positionAToBInA, rotationBToA);
  Pose poseBToA2(positionAToBInA, rotationBToA);
  Pose poseCToA(positionAToCInA, rotationCToA);

  // Check equality comparison
  ASSERT_TRUE(poseBToA == poseBToA2);
  ASSERT_FALSE(poseBToA == poseCToA);
}

TYPED_TEST(HomogeneousTransformationTest, testComparisonNotEqual)
{
  typedef typename TestFixture::Pose Pose;
  typedef typename TestFixture::Position Position;
  typedef typename TestFixture::Rotation Rotation;
  typedef typename TestFixture::Scalar Scalar;
  Position positionAToBInA(1.0,2.0,3.0);
  Position positionAToCInA(1.0,2.0,4.0);
  Rotation rotationBToA(kindr::EulerAnglesZyx<Scalar>(0.5, -0.9, 1.2));
  Rotation rotationCToA(kindr::EulerAnglesZyx<Scalar>(0.5, -0.9, 1.2));
  Pose poseBToA(positionAToBInA, rotationBToA);
  Pose poseBToA2(positionAToBInA, rotationBToA);
  Pose poseCToA(positionAToCInA, rotationCToA);

  // Check inequality comparison
  ASSERT_FALSE(poseBToA != poseBToA2);
  ASSERT_TRUE(poseBToA != poseCToA);
}

TYPED_TEST(HomogeneousTransformationTest, testTransform)
{
  typedef typename TestFixture::Pose Pose;
  typedef typename TestFixture::Position Position;
  typedef typename TestFixture::Rotation Rotation;
  typedef typename TestFixture::Scalar Scalar;
  Position positionAToBInA(1.0,2.0,3.0);
  Rotation rotationBToA(kindr::EulerAnglesZyx<Scalar>(0.5, -0.9, 1.2));
  Pose poseBToA(positionAToBInA, rotationBToA);
  Position positionInB(0.5, 0.4, -5.4);
  Position positionInA = poseBToA.transform(positionInB);
  Position expectedPositionInA = rotationBToA.rotate(positionInB) + positionAToBInA;

  EXPECT_NEAR(expectedPositionInA.x(), positionInA.x(), 1.0e-6);
  EXPECT_NEAR(expectedPositionInA.y(), positionInA.y(), 1.0e-6);
  EXPECT_NEAR(expectedPositionInA.z(), positionInA.z(), 1.0e-6);
}

TYPED_TEST(HomogeneousTransformationTest, testInverseTransform)
{
  typedef typename TestFixture::Pose Pose;
  typedef typename TestFixture::Position Position;
  typedef typename TestFixture::Rotation Rotation;
  typedef typename TestFixture::Scalar Scalar;
  Position positionAToBInA(1.0,2.0,3.0);
  Rotation rotationBToA(kindr::EulerAnglesZyx<Scalar>(0.5, -0.9, 1.2));
  Pose poseBToA(positionAToBInA, rotationBToA);
  Position positionInA(0.5, 0.4, -5.4);
  Position positionInB = poseBToA.inverseTransform(positionInA);
  Position expectedPositionInB = rotationBToA.inverseRotate(positionInA - positionAToBInA);

  EXPECT_NEAR(expectedPositionInB.x(), positionInB.x(), 1.0e-6);
  EXPECT_NEAR(expectedPositionInB.y(), positionInB.y(), 1.0e-6);
  EXPECT_NEAR(expectedPositionInB.z(), positionInB.z(), 1.0e-6);
}

TYPED_TEST(HomogeneousTransformationTest, testTransformAndInverseTransform)
{
  typedef typename TestFixture::Pose Pose;
  typedef typename TestFixture::Position Position;
  typedef typename TestFixture::Rotation Rotation;
  typedef typename TestFixture::Scalar Scalar;
  Position positionAToBInA(1.0,2.0,3.0);
  Rotation rotationBToA(kindr::EulerAnglesZyx<Scalar>(0.5, -0.9, 1.2));
  Pose poseBToA(positionAToBInA, rotationBToA);
  Position positionInB(0.5, 0.4, -5.4);
  Position positionInA = poseBToA.transform(positionInB);
  Position positionInBNew = poseBToA.inverseTransform(positionInA);
  EXPECT_NEAR(positionInB.x(), positionInBNew.x(), 1.0e-6);
  EXPECT_NEAR(positionInB.y(), positionInBNew.y(), 1.0e-6);
  EXPECT_NEAR(positionInB.z(), positionInBNew.z(), 1.0e-6);
}

TYPED_TEST(HomogeneousTransformationTest, testConcatenation)
{
  typedef typename TestFixture::Pose Pose;
  typedef typename TestFixture::Position Position;
  typedef typename TestFixture::Rotation Rotation;
  typedef typename TestFixture::Scalar Scalar;
  Position positionAToBInA(0.5, -0.8, 1.3);
  Rotation rotationBToA(kindr::EulerAnglesZyx<Scalar>(0.4, 0.6, -2.5));
  Pose poseBToA(positionAToBInA, rotationBToA);

  Position positionBToCInB(5.7, 1.3, -5.1);
  Rotation rotationCToB(kindr::EulerAnglesZyx<Scalar>(-0.9, 2.1, 7.9));
  Pose poseCToB(positionBToCInB, rotationCToB);

  Position positionInC(-0.8, 5.0, 1.6);

  Pose poseCToA = poseBToA*poseCToB;

  Position positionConcatTransform = poseCToA.transform(positionInC);
  Position positionTransform = (rotationBToA*rotationCToB).rotate(positionInC) +  (positionAToBInA + rotationBToA.rotate(positionBToCInB));
  EXPECT_NEAR(positionTransform.x(), positionConcatTransform.x(), 1.0e-3);
  EXPECT_NEAR(positionTransform.y(), positionConcatTransform.y(), 1.0e-3);
  EXPECT_NEAR(positionTransform.z(), positionConcatTransform.z(), 1.0e-3);
}

TYPED_TEST(HomogeneousTransformationTest, testGenericRotateVectorCompilable)
{
  typedef typename TestFixture::Pose Pose;
  typedef typename TestFixture::Position Position;
  typedef typename TestFixture::Rotation Rotation;
  typedef typename TestFixture::Scalar Scalar;

  Pose test(Position(1,2,3),Rotation(kindr::AngleAxis<Scalar>(0.5,1.0,0,0)));

  kindr::Velocity<Scalar, 3> vel(-1,2,3);
  test.getRotation().rotate(vel);
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm core if_
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/feature/core/data_customization_point/array.h"
#include "fern/feature/core/data_customization_point/masked_array.h"
#include "fern/feature/core/data_customization_point/masked_scalar.h"
#include "fern/algorithm/algebra/boole/or.h"
#include "fern/algorithm/core/if.h"
#include "fern/algorithm/core/test/test_utils.h"

namespace fa = fern::algorithm;

template<
    typename ExecutionPolicy>
void test_array_0d_2d(
    ExecutionPolicy& execution_policy)
{
    size_t const nr_threads{fern::hardware_concurrency()};
    size_t const nr_rows{30 * nr_threads};
    size_t const nr_cols{20 * nr_threads};
    size_t const nr_elements{nr_rows * nr_cols};

    int condition;
    fern::Array<int, 2> true_value(fern::extents[nr_rows][nr_cols]);
    fern::Array<int, 2> result_we_want(fern::extents[nr_rows][nr_cols]);
    fern::Array<int, 2> result_we_got(fern::extents[nr_rows][nr_cols]);

    // Fill true_value: 0, 1, 2, ...
    std::iota(true_value.data(), true_value.data() + nr_elements, 0);

    {
        condition = 0;

        // Condition is false: result_we_got is not touched.
        result_we_want.fill(-9);
        result_we_got.fill(-9);

        fa::core::if_(execution_policy, condition, true_value, result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    {
        condition = 1;

        // Fill result_we_want: copy true_value.
        std::copy(true_value.data(), true_value.data() + nr_elements,
            result_we_want.data());

        result_we_got.fill(-9);
        fa::core::if_(execution_policy, condition, true_value, result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

}

BOOST_AUTO_TEST_CASE(array_0d_2d_sequential)
{
    fa::SequentialExecutionPolicy sequential;

    test_array_0d_2d(sequential);
    fa::ExecutionPolicy execution_policy{sequential};
    test_array_0d_2d(execution_policy);
}

BOOST_AUTO_TEST_CASE(array_0d_2d_parallel)
{
    fa::ParallelExecutionPolicy parallel;

    test_array_0d_2d(parallel);
    fa::ExecutionPolicy execution_policy{parallel};
    test_array_0d_2d(execution_policy);
}

template<
    typename ExecutionPolicy>
void test_array_0d_2d_masked(
    ExecutionPolicy& execution_policy)
{
    size_t const nr_threads{fern::hardware_concurrency()};
    size_t const nr_rows{30 * nr_threads};
    size_t const nr_cols{20 * nr_threads};
    size_t const nr_elements{nr_rows * nr_cols};

    fern::MaskedScalar<int> condition;
    fern::MaskedArray<int, 2> true_value(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> result_we_want(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> result_we_got(fern::extents[nr_rows][nr_cols]);

    fa::InputNoDataPolicies<fa::DetectNoDataByValue<bool>,
        fa::DetectNoDataByValue<fern::Mask<2>>> input_no_data_policy{
            {condition.mask(), true},
            {true_value.mask(), true}};
    fa::MarkNoDataByValue<fern::Mask<2>> output_no_data_policy(
        result_we_got.mask(), true);

    {
        // Fill true_value:
        // - 0, 1, 2, ...
        // - All mask cells with index % 7 become masked, all other mask cells
        //   don't.
        std::iota(true_value.data(), true_value.data() + nr_elements, 0);
        int n{0};
        std::generate(true_value.mask().data(), true_value.mask().data() +
            nr_elements, [&] () { return n++ % 7 == 0 ? true : false; });
    }

    // Condition is not masked and false.
    {
        condition = 0;
        condition.mask() = false;

        // Condition is false: result_we_want is masked.
        result_we_want.mask().fill(true);
        result_we_want.fill(-9);
        result_we_got.mask().fill(false);
        result_we_got.fill(-9);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    // Condition is not masked and true.
    {
        condition = 1;
        condition.mask() = false;

        // Fill result_we_want: copy true_value.

        // Fill result_we_want.mask().
        // If true_value is masked, the result must be masked.
        result_we_want.mask().fill(false);
        fa::algebra::or_(execution_policy, true_value.mask(),
            result_we_want.mask(), result_we_want.mask());

        // Fill result_we_want.
        // Copy true_value if mask is false, else -9.
        auto mask_it = result_we_want.mask().data();
        std::transform(true_value.data(), true_value.data() + nr_elements,
            result_we_want.data(), [&](int const& value) {
                ++mask_it;
                return *(mask_it-1) == 0 ? value : -9;
            });

        result_we_got.mask().fill(false);
        result_we_got.fill(-9);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    // Condition is masked.
    {
        condition = 1;
        condition.mask() = true;

        // Result we want is fully masked.
        result_we_want.fill(-9);
        result_we_want.mask().fill(true);

        result_we_got.fill(-9);
        result_we_got.mask().fill(false);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

}

BOOST_AUTO_TEST_CASE(array_0d_2d_sequential_masked)
{
    fa::SequentialExecutionPolicy sequential;

    test_array_0d_2d_masked(sequential);
    fa::ExecutionPolicy execution_policy{sequential};
    test_array_0d_2d_masked(execution_policy);
}

BOOST_AUTO_TEST_CASE(array_0d_2d_parallel_masked)
{
    fa::ParallelExecutionPolicy parallel;

    test_array_0d_2d_masked(parallel);
    fa::ExecutionPolicy execution_policy{parallel};
    test_array_0d_2d_masked(execution_policy);
}

template<
    typename ExecutionPolicy>
void test_array_0d_0d_2d_masked(
    ExecutionPolicy& execution_policy)
{
    size_t const nr_threads{fern::hardware_concurrency()};
    size_t const nr_rows{30 * nr_threads};
    size_t const nr_cols{20 * nr_threads};
    size_t const nr_elements{nr_rows * nr_cols};

    fern::MaskedScalar<int> condition;
    fern::MaskedScalar<int> true_value;
    fern::MaskedArray<int, 2> false_value(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> result_we_want(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> result_we_got(fern::extents[nr_rows][nr_cols]);

    fa::InputNoDataPolicies<fa::DetectNoDataByValue<bool>,
        fa::DetectNoDataByValue<bool>,
        fa::DetectNoDataByValue<fern::Mask<2>>> input_no_data_policy{
            {condition.mask(), true},
            {true_value.mask(), true},
            {false_value.mask(), true}};
    fa::MarkNoDataByValue<fern::Mask<2>> output_no_data_policy(
        result_we_got.mask(), true);

    {
        // Fill false_value:
        // - 10, 11, 12, ...
        // - All mask cells with index % 9 become masked, all other mask cells
        //   don't.
        std::iota(false_value.data(), false_value.data() + nr_elements, 10);
        int n{0};
        std::generate(false_value.mask().data(), false_value.mask().data() +
            nr_elements, [&] () { return n++ % 9 == 0 ? true : false; });
    }

    // Condition is not masked and false.
    {
        condition = 0;
        condition.mask() = false;

        // Fill result_we_want: copy false_value.

        // Fill result_we_want.mask().
        // If false_value is masked, the result must be masked.
        result_we_want.mask().fill(false);
        fa::algebra::or_(execution_policy, false_value.mask(),
            result_we_want.mask(), result_we_want.mask());

        // Fill result_we_want.
        // Copy false_value if mask is false, else -9.
        auto mask_it = result_we_want.mask().data();
        std::transform(false_value.data(), false_value.data() + nr_elements,
            result_we_want.data(), [&](int const& value) {
                ++mask_it;
                return *(mask_it-1) == 0 ? value : -9;
            });

        result_we_got.mask().fill(false);
        result_we_got.fill(-9);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    // Condition is not masked and true.
    {
        condition = 1;
        condition.mask() = false;

        // true_value is masked.
        // Result is masked.
        true_value.mask() = true;

        result_we_want.fill(-9);
        result_we_want.mask().fill(true);

        result_we_got.fill(-9);
        result_we_got.mask().fill(false);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));

        // true_value is not masked.
        // Copy true_value.
        true_value.mask() = false;

        result_we_want.fill(true_value.value());
        result_we_want.mask().fill(false);

        result_we_got.fill(-9);
        result_we_got.mask().fill(false);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    // Condition is masked.
    {
        condition = 1;
        condition.mask() = true;

        // Result we want is fully masked.
        result_we_want.fill(-9);
        result_we_want.mask().fill(true);

        result_we_got.fill(-9);
        result_we_got.mask().fill(false);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }
}

BOOST_AUTO_TEST_CASE(array_0d_0d_2d_sequential_masked)
{
    fa::SequentialExecutionPolicy sequential;

    test_array_0d_0d_2d_masked(sequential);
    fa::ExecutionPolicy execution_policy{sequential};
    test_array_0d_0d_2d_masked(execution_policy);
}

BOOST_AUTO_TEST_CASE(array_0d_0d_2d_parallel_masked)
{
    fa::ParallelExecutionPolicy parallel;

    test_array_0d_0d_2d_masked(parallel);
    fa::ExecutionPolicy execution_policy{parallel};
    test_array_0d_0d_2d_masked(execution_policy);
}

template<
    typename ExecutionPolicy>
void test_array_0d_2d_0d_masked(
    ExecutionPolicy& execution_policy)
{
    size_t const nr_threads{fern::hardware_concurrency()};
    size_t const nr_rows{30 * nr_threads};
    size_t const nr_cols{20 * nr_threads};
    size_t const nr_elements{nr_rows * nr_cols};

    fern::MaskedScalar<int> condition;
    fern::MaskedArray<int, 2> true_value(fern::extents[nr_rows][nr_cols]);
    fern::MaskedScalar<int> false_value;
    fern::MaskedArray<int, 2> result_we_want(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> result_we_got(fern::extents[nr_rows][nr_cols]);

    fa::InputNoDataPolicies<fa::DetectNoDataByValue<bool>,
        fa::DetectNoDataByValue<fern::Mask<2>>,
        fa::DetectNoDataByValue<bool>> input_no_data_policy{
            {condition.mask(), true},
            {true_value.mask(), true},
            {false_value.mask(), true}};
    fa::MarkNoDataByValue<fern::Mask<2>> output_no_data_policy(
        result_we_got.mask(), true);

    {
        // Fill true_value:
        // - 0, 1, 2, ...
        // - All mask cells with index % 7 become masked, all other mask cells
        //   don't.
        std::iota(true_value.data(), true_value.data() + nr_elements, 0);
        int n{0};
        std::generate(true_value.mask().data(), true_value.mask().data() +
            nr_elements, [&] () { return n++ % 7 == 0 ? true : false; });
    }

    // Condition is not masked and false.
    {
        condition = 0;
        condition.mask() = false;

        // false_value is masked.
        // Result is masked.
        false_value.mask() = true;

        result_we_want.fill(-9);
        result_we_want.mask().fill(true);

        result_we_got.fill(-9);
        result_we_got.mask().fill(false);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));

        // false_value is not masked.
        // Copy false_value.
        false_value.mask() = false;

        result_we_want.fill(false_value.value());
        result_we_want.mask().fill(false);

        result_we_got.fill(-9);
        result_we_got.mask().fill(false);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    // Condition is not masked and true.
    {
        condition = 1;
        condition.mask() = false;

        // Fill result_we_want: copy false_value.

        // Fill result_we_want.mask().
        // If true_value is masked, the result must be masked.
        result_we_want.mask().fill(false);
        fa::algebra::or_(execution_policy, true_value.mask(),
            result_we_want.mask(), result_we_want.mask());

        // Fill result_we_want.
        // Copy true_value if mask is false, else -9.
        auto mask_it = result_we_want.mask().data();
        std::transform(true_value.data(), true_value.data() + nr_elements,
            result_we_want.data(), [&](int const& value) {
                ++mask_it;
                return *(mask_it-1) == 0 ? value : -9;
            });

        result_we_got.mask().fill(false);
        result_we_got.fill(-9);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    // Condition is masked.
    {
        condition = 1;
        condition.mask() = true;

        // Result we want is fully masked.
        result_we_want.fill(-9);
        result_we_want.mask().fill(true);

        result_we_got.fill(-9);
        result_we_got.mask().fill(false);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }
}

BOOST_AUTO_TEST_CASE(array_0d_2d_0d_sequential_masked)
{
    fa::SequentialExecutionPolicy sequential;

    test_array_0d_2d_0d_masked(sequential);
    fa::ExecutionPolicy execution_policy{sequential};
    test_array_0d_2d_0d_masked(execution_policy);
}

BOOST_AUTO_TEST_CASE(array_0d_2d_0d_parallel_masked)
{
    fa::ParallelExecutionPolicy parallel;

    test_array_0d_2d_0d_masked(parallel);
    fa::ExecutionPolicy execution_policy{parallel};
    test_array_0d_2d_0d_masked(execution_policy);
}

template<
    typename ExecutionPolicy>
void test_array_0d_2d_2d_masked(
    ExecutionPolicy& execution_policy)
{
    size_t const nr_threads{fern::hardware_concurrency()};
    size_t const nr_rows{30 * nr_threads};
    size_t const nr_cols{20 * nr_threads};
    size_t const nr_elements{nr_rows * nr_cols};

    fern::MaskedScalar<int> condition;
    fern::MaskedArray<int, 2> true_value(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> false_value(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> result_we_want(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> result_we_got(fern::extents[nr_rows][nr_cols]);

    fa::InputNoDataPolicies<fa::DetectNoDataByValue<bool>,
        fa::DetectNoDataByValue<fern::Mask<2>>,
        fa::DetectNoDataByValue<fern::Mask<2>>> input_no_data_policy{
            {condition.mask(), true},
            {true_value.mask(), true},
            {false_value.mask(), true}};
    fa::MarkNoDataByValue<fern::Mask<2>> output_no_data_policy(
        result_we_got.mask(), true);

    {
        // Fill true_value:
        // - 0, 1, 2, ...
        // - All mask cells with index % 7 become masked, all other mask cells
        //   don't.
        std::iota(true_value.data(), true_value.data() + nr_elements, 0);
        int n{0};
        std::generate(true_value.mask().data(), true_value.mask().data() +
            nr_elements, [&] () { return n++ % 7 == 0 ? true : false; });

        // Fill false_value:
        // - 10, 11, 12, ...
        // - All mask cells with index % 9 become masked, all other mask cells
        //   don't.
        std::iota(false_value.data(), false_value.data() + nr_elements, 10);
        n = 0;
        std::generate(false_value.mask().data(), false_value.mask().data() +
            nr_elements, [&] () { return n++ % 9 == 0 ? true : false; });
    }

    // Condition is not masked and false.
    {
        condition = 0;
        condition.mask() = false;

        // Fill result_we_want: copy false_value.

        // Fill result_we_want.mask().
        // If false_value is masked, the result must be masked.
        result_we_want.mask().fill(false);
        fa::algebra::or_(execution_policy, false_value.mask(),
            result_we_want.mask(), result_we_want.mask());

        // Fill result_we_want.
        // Copy false_value if mask is false, else -9.
        auto mask_it = result_we_want.mask().data();
        std::transform(false_value.data(), false_value.data() + nr_elements,
            result_we_want.data(), [&](int const& value) {
                ++mask_it;
                return *(mask_it-1) == 0 ? value : -9;
            });

        result_we_got.mask().fill(false);
        result_we_got.fill(-9);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    // Condition is not masked and true.
    {
        condition = 1;
        condition.mask() = false;

        // Fill result_we_want: copy true_value.

        // Fill result_we_want.mask().
        // If true_value is masked, the result must be masked.
        result_we_want.mask().fill(false);
        fa::algebra::or_(execution_policy, true_value.mask(),
            result_we_want.mask(), result_we_want.mask());

        // Fill result_we_want.
        // Copy true_value if mask is false, else -9.
        auto mask_it = result_we_want.mask().data();
        std::transform(true_value.data(), true_value.data() + nr_elements,
            result_we_want.data(), [&](int const& value) {
                ++mask_it;
                return *(mask_it-1) == 0 ? value : -9;
            });

        result_we_got.mask().fill(false);
        result_we_got.fill(-9);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    // Condition is masked.
    {
        condition = 1;
        condition.mask() = true;

        // Result we want is fully masked.
        result_we_want.fill(-9);
        result_we_want.mask().fill(true);

        result_we_got.fill(-9);
        result_we_got.mask().fill(false);

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

}

BOOST_AUTO_TEST_CASE(array_0d_2d_2d_sequential_masked)
{
    fa::SequentialExecutionPolicy sequential;

    test_array_0d_2d_2d_masked(sequential);
    fa::ExecutionPolicy execution_policy{sequential};
    test_array_0d_2d_2d_masked(execution_policy);
}

BOOST_AUTO_TEST_CASE(array_0d_2d_2d_parallel_masked)
{
    fa::ParallelExecutionPolicy parallel;

    test_array_0d_2d_2d_masked(parallel);
    fa::ExecutionPolicy execution_policy{parallel};
    test_array_0d_2d_2d_masked(execution_policy);
}

template<
    typename ExecutionPolicy>
void test_array_0d_0d_0d(
    ExecutionPolicy& execution_policy)
{
    int condition;
    int true_value;
    int false_value;
    int result_we_want;
    int result_we_got;

    {
        condition = 0;
        true_value = 5;
        false_value = 6;

        result_we_want = -9;
        result_we_got = -9;
        fa::core::if_(execution_policy, condition, true_value, result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));

        result_we_want = 6;
        result_we_got = -9;
        fa::core::if_(execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    {
        condition = 1;
        true_value = 5;
        false_value = 6;

        result_we_want = 5;
        result_we_got = -9;
        fa::core::if_(execution_policy, condition, true_value, result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));

        result_we_want = 5;
        result_we_got = -9;
        fa::core::if_(execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

}

BOOST_AUTO_TEST_CASE(array_0d_0d_0d_sequential)
{
    fa::SequentialExecutionPolicy sequential;

    test_array_0d_0d_0d(sequential);
    fa::ExecutionPolicy execution_policy{sequential};
    test_array_0d_0d_0d(execution_policy);
}

BOOST_AUTO_TEST_CASE(array_0d_0d_0d_parallel)
{
    fa::ParallelExecutionPolicy parallel;

    test_array_0d_0d_0d(parallel);
    fa::ExecutionPolicy execution_policy{parallel};
    test_array_0d_0d_0d(execution_policy);
}

template<
    typename ExecutionPolicy>
void test_array_0d_0d_0d_masked(
    ExecutionPolicy& execution_policy)
{
    fern::MaskedScalar<int> condition;
    fern::MaskedScalar<int> true_value;
    fern::MaskedScalar<int> false_value;
    fern::MaskedScalar<int> result_we_want;
    fern::MaskedScalar<int> result_we_got;

    fa::MarkNoDataByValue<bool> output_no_data_policy(result_we_got.mask(),
        true);

    {
        fa::InputNoDataPolicies<fa::DetectNoDataByValue<bool>,
            fa::DetectNoDataByValue<bool>, fa::DetectNoDataByValue<bool>>
                input_no_data_policy{
                    {condition.mask(), true},
                    {true_value.mask(), true},
                    {false_value.mask(), true}};

        condition = 0;
        condition.mask() = false;
        true_value = 5;
        true_value.mask() = false;
        false_value = 6;
        false_value.mask() = false;

        result_we_want = fern::MaskedScalar<int>(-9, true);
        result_we_got = fern::MaskedScalar<int>(-9, false);
        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));

        result_we_want = fern::MaskedScalar<int>(6, false);
        result_we_got = fern::MaskedScalar<int>(-9, false);
        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    {
        fa::InputNoDataPolicies<fa::DetectNoDataByValue<bool>,
            fa::DetectNoDataByValue<bool>, fa::DetectNoDataByValue<bool>>
                input_no_data_policy{
                    {condition.mask(), true},
                    {true_value.mask(), true},
                    {false_value.mask(), true}};

        condition = 1;
        condition.mask() = false;
        true_value = 5;
        true_value.mask() = false;
        false_value = 6;
        false_value.mask() = false;

        result_we_want = fern::MaskedScalar<int>(5, false);
        result_we_got = fern::MaskedScalar<int>(-9, false);
        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, result_we_got);
        BOOST_CHECK_EQUAL(result_we_got.value(), 5);
        BOOST_CHECK(!result_we_got.mask());

        result_we_want = fern::MaskedScalar<int>(5, false);
        result_we_got = fern::MaskedScalar<int>(-9, false);
        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK_EQUAL(result_we_got.value(), 5);
        BOOST_CHECK(!result_we_got.mask());
    }
}

BOOST_AUTO_TEST_CASE(array_0d_0d_0d_masked_sequential)
{
    fa::SequentialExecutionPolicy sequential;

    test_array_0d_0d_0d_masked(sequential);
    fa::ExecutionPolicy execution_policy{sequential};
    test_array_0d_0d_0d_masked(execution_policy);
}

BOOST_AUTO_TEST_CASE(array_0d_0d_0d_masked_parallel)
{
    fa::ParallelExecutionPolicy parallel;

    test_array_0d_0d_0d_masked(parallel);
    fa::ExecutionPolicy execution_policy{parallel};
    test_array_0d_0d_0d_masked(execution_policy);
}

// TODO 1d

template<
    typename ExecutionPolicy>
void test_array_2d_2d_2d(
    ExecutionPolicy& execution_policy)
{
    size_t const nr_threads{fern::hardware_concurrency()};
    size_t const nr_rows{30 * nr_threads};
    size_t const nr_cols{20 * nr_threads};
    size_t const nr_elements{nr_rows * nr_cols};

    fern::Array<int, 2> condition(fern::extents[nr_rows][nr_cols]);
    fern::Array<int, 2> true_value(fern::extents[nr_rows][nr_cols]);
    fern::Array<int, 2> false_value(fern::extents[nr_rows][nr_cols]);
    fern::Array<int, 2> result_we_got(fern::extents[nr_rows][nr_cols]);

    {
        // Fill condition: all cells with odd indices become true cells, all
        // even cells become false cells.
        int n{0};
        std::generate(condition.data(), condition.data() + nr_elements,
            [&] () { return n++ % 2 == 0 ? 0 : 1; });

        // Fill true_value: 0, 1, 2, ...
        std::iota(true_value.data(), true_value.data() + nr_elements, 0);

        // Fill false_value: 10, 11, 12, ...
        std::iota(false_value.data(), false_value.data() + nr_elements, 10);
    }

    // if_then
    {
        fern::Array<int, 2> result_we_want(fern::extents[nr_rows][nr_cols]);

        {
            // Fill result_we_want: copy true_value if condition it true, else
            // -9.
            auto true_value_it = true_value.data();
            std::transform(condition.data(), condition.data() + nr_elements,
                result_we_want.data(), [&](int const& value) {
                    ++true_value_it;
                    return value ? *(true_value_it-1) : -9; });
        }

        result_we_got.fill(-9);
        fa::core::if_(execution_policy, condition, true_value, result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    // if_then_else
    {
        fern::Array<int, 2> result_we_want(fern::extents[nr_rows][nr_cols]);

        {
            // Fill result_we_want: copy true_value if condition it true, else
            // false_value.
            auto true_value_it = true_value.data();
            auto false_value_it = false_value.data();
            std::transform(condition.data(), condition.data() + nr_elements,
                result_we_want.data(), [&] (int const& value) {
                    ++true_value_it;
                    ++false_value_it;
                    return value ? *(true_value_it-1) : *(false_value_it-1); });
        }

        result_we_got.fill(-9);
        fa::core::if_(execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }
}

BOOST_AUTO_TEST_CASE(array_2d_2d_2d_sequential)
{
    fa::SequentialExecutionPolicy sequential;

    test_array_2d_2d_2d(sequential);
    fa::ExecutionPolicy execution_policy{sequential};
    test_array_2d_2d_2d(execution_policy);
}

BOOST_AUTO_TEST_CASE(array_2d_2d_2d_parallel)
{
    fa::ParallelExecutionPolicy parallel;

    test_array_2d_2d_2d(parallel);
    fa::ExecutionPolicy execution_policy{parallel};
    test_array_2d_2d_2d(execution_policy);
}

template<
    typename ExecutionPolicy>
void test_array_2d_2d_2d_masked(
    ExecutionPolicy& execution_policy)
{
    size_t const nr_threads{fern::hardware_concurrency()};
    size_t const nr_rows{30 * nr_threads};
    size_t const nr_cols{20 * nr_threads};
    size_t const nr_elements{nr_rows * nr_cols};

    fern::MaskedArray<int, 2> condition(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> true_value(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> false_value(fern::extents[nr_rows][nr_cols]);
    fern::MaskedArray<int, 2> result_we_got(fern::extents[nr_rows][nr_cols]);

    fa::MarkNoDataByValue<fern::Mask<2>> output_no_data_policy(
        result_we_got.mask(), true);

    {
        // Fill condition:
        // - All cells with index % 2 become true cells, all other cells
        //   become false cells.
        // - All mask cells with index % 5 become masked, all other mask cells
        //   don't.
        int n{0};
        std::generate(condition.data(), condition.data() + nr_elements,
            [&] () { return n++ % 2 == 0 ? 0 : 1; });
        n = 0;
        std::generate(condition.mask().data(), condition.mask().data() +
            nr_elements, [&] () { return n++ % 5 == 0 ? true : false; });

        // Fill true_value:
        // - 0, 1, 2, ...
        // - All mask cells with index % 7 become masked, all other mask cells
        //   don't.
        std::iota(true_value.data(), true_value.data() + nr_elements, 0);
        n = 0;
        std::generate(true_value.mask().data(), true_value.mask().data() +
            nr_elements, [&] () { return n++ % 7 == 0 ? true : false; });

        // Fill false_value:
        // - 10, 11, 12, ...
        // - All mask cells with index % 9 become masked, all other mask cells
        //   don't.
        std::iota(false_value.data(), false_value.data() + nr_elements, 10);
        n = 0;
        std::generate(false_value.mask().data(), false_value.mask().data() +
            nr_elements, [&] () { return n++ % 9 == 0 ? true : false; });
    }

    // if_then
    {
        fa::InputNoDataPolicies<fa::DetectNoDataByValue<fern::Mask<2>>,
            fa::DetectNoDataByValue<fern::Mask<2>>> input_no_data_policy{
                {condition.mask(), true},
                {true_value.mask(), true}};

        fern::MaskedArray<int, 2> result_we_want(
            fern::extents[nr_rows][nr_cols]);

        {
            // Fill result_we_want.mask().
            // If condition is false, the result must be masked.
            // If condition is masked, the result must be masked.
            // If true_value is masked, the result must be masked.
            std::transform(condition.data(), condition.data() + nr_elements,
                result_we_want.mask().data(), [&](int const& value) {
                    return value ? false : true; });
            fa::algebra::or_(execution_policy, condition.mask(),
                result_we_want.mask(), result_we_want.mask());
            fa::algebra::or_(execution_policy, true_value.mask(),
                result_we_want.mask(), result_we_want.mask());

            // Fill result_we_want.
            // Copy true_value if condition it true and mask is false, else -9.
            auto true_value_it = true_value.data();
            auto mask_it = result_we_want.mask().data();
            std::transform(condition.data(), condition.data() + nr_elements,
                result_we_want.data(), [&](int const& value) {
                    ++true_value_it;
                    ++mask_it;
                    return value && *(mask_it-1) == 0 ? *(true_value_it-1)
                        : -9; });
        }

        result_we_got.fill(-9);
        result_we_got.mask().fill(false);
        fa::algebra::or_(execution_policy, condition.mask(),
            true_value.mask(), result_we_got.mask());

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }

    // if_then_else
    {
        fa::InputNoDataPolicies<fa::DetectNoDataByValue<fern::Mask<2>>,
            fa::DetectNoDataByValue<fern::Mask<2>>,
            fa::DetectNoDataByValue<fern::Mask<2>>> input_no_data_policy{
                {condition.mask(), true},
                {true_value.mask(), true},
                {false_value.mask(), true}};

        fern::MaskedArray<int, 2> result_we_want(
            fern::extents[nr_rows][nr_cols]);

        {
            // Fill result_we_want.mask().
            // - If condition is masked, the result must be masked.
            // - If condition is true and true_value is masked, the result
            //   must be masked.
            // - If condition is false and false_value is masked, the result
            //   must be masked.
            //
            // TODO It would be nice to be able to replace this loop with
            //      a nested expression of our own.
            for(size_t i = 0; i < size(result_we_want); ++i) {
                if(get(condition.mask(), i) ||
                        (get(true_value, i) && get(true_value.mask(), i)) ||
                        (get(false_value, i) && get(false_value.mask(), i))) {
                    get(result_we_want.mask(), i) = true;
                }
            }

            // - Initialize all values with -9.
            // - If condition is not no-data and true and true_value is not
            //   masked, the result must be true_value.
            // - If condition is not no-data and false and false_value is not
            //   masked, the result must be false_value.
            //
            // TODO It would be nice to be able to replace this loop with
            //      a nested expression of our own.
            result_we_want.fill(-9);
            for(size_t i = 0; i < size(result_we_want); ++i) {
                if(!get(condition.mask(), i)) {
                    if(get(condition, i)) {
                        if(!get(true_value.mask(), i)) {
                            get(result_we_want, i) = get(true_value, i);
                        }
                    }
                    else {
                        if(!get(false_value.mask(), i)) {
                            get(result_we_want, i) = get(false_value, i);
                        }
                    }
                }
            }
        }

        result_we_got.fill(-9);
        result_we_got.mask().fill(false);
        fa::algebra::or_(execution_policy, result_we_got.mask(),
            condition.mask(), result_we_got.mask());
        fa::algebra::or_(execution_policy, result_we_got.mask(),
            true_value.mask(), result_we_got.mask());
        fa::algebra::or_(execution_policy, result_we_got.mask(),
            false_value.mask(), result_we_got.mask());

        fa::core::if_(input_no_data_policy, output_no_data_policy,
            execution_policy, condition, true_value, false_value,
            result_we_got);
        BOOST_CHECK(fern::compare(execution_policy, result_we_got,
            result_we_want));
    }
}

BOOST_AUTO_TEST_CASE(array_2d_2d_2d_masked_sequential)
{
    fa::SequentialExecutionPolicy sequential;

    test_array_2d_2d_2d_masked(sequential);
    fa::ExecutionPolicy execution_policy{sequential};
    test_array_2d_2d_2d_masked(execution_policy);
}

BOOST_AUTO_TEST_CASE(array_2d_2d_2d_masked_parallel)
{
    fa::ParallelExecutionPolicy parallel;

    test_array_2d_2d_2d_masked(parallel);
    fa::ExecutionPolicy execution_policy{parallel};
    test_array_2d_2d_2d_masked(execution_policy);
}


/**
 * @file demo.hpp
 * @author guoqing (1337841346@qq.com)
 * @brief 实例demo.h
 * @version 0.1
 * @date 2019-08-08
 *
 * @copyright Copyright (c) 2019
 *
 */

#ifndef __DEMO_HPP__
#define __DEMO_HPP__

#include <vector>
#include <pangolin/pangolin.h>
#include <Eigen/Core>

// 一个转换函数
std::vector<GLfloat> eigen2glfloat(Eigen::Isometry3d T)
{
    //注意是列优先
    std::vector<GLfloat> res;
    for(int j=0;j<4;j++)
    {
        for(int i=0;i<4;i++)
        {
            res.push_back(T(i,j));
        }
    }
    return res;
}

void demoDrawPoint(void)
{
    // 设置点大小和颜色
    glPointSize(5.0f);
    glColor3f(1.0,1.0,1.0);
    // 每次绘制前都要有这个; GL_POINTS 表示当前绘制点
    // ref: https://blog.csdn.net/aa941096979/article/details/50843596
    glBegin(GL_POINTS);
    // 原点
    glVertex3f(0.0f,0.0f,0.0f);
    // x轴上的一个点
    glVertex3f(0.5,0,0);
    // y轴上的一个点
    glVertex3f(0,1,0);
    glEnd();
}

void demoPolygon(void)
{
    glColor3f(0.0, 0.54,0.54);
    glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
    glBegin(GL_POLYGON);
    glVertex3f(2.25,0.25,0.4);
    glVertex3f(2.75,2.25,0.4);
    glVertex3f(2.75,0.75,-0.9);
    glVertex3f(5.0,0.75,0.4);
    glEnd();
    glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
}

void drawOctahedronFrame(double size, double line_width, Eigen::Isometry3d& Twc)
{
    // 先绘制图像,再转移变换到任意位置的示例
    std::vector<GLfloat> glTwc=eigen2glfloat(Twc);

    // 绘制参数, 正方形边长 2a, 高度b
    const double a=size;
    const double b=a*sqrt(2);

    // 要和后面的 popMatrix 成对出现
    glPushMatrix();
    glMultMatrixf((GLfloat*)glTwc.data());

    glLineWidth(line_width);
    // 颜色
    glColor3f(0.8f,0.54f,0.78f);
    // 绘制直线,每两个点组成一个线段
    glBegin(GL_LINES);

    // 绘制正方形
    glVertex3f(a,a,0);
    glVertex3f(a,-a,0);
    glVertex3f(a,-a,0);
    glVertex3f(-a,-a,0);
    glVertex3f(-a,-a,0);
    glVertex3f(-a,a,0);
    glVertex3f(-a,a,0);
    glVertex3f(a,a,0);

    // 连接上部顶点
    glVertex3f(a,a,0);
    glVertex3f(0,0,b);
    glVertex3f(a,-a,0);
    glVertex3f(0,0,b);
    glVertex3f(-a,-a,0);
    glVertex3f(0,0,b);
    glVertex3f(-a,a,0);
    glVertex3f(0,0,b);

    // 连接下部顶点
    glVertex3f(a,a,0);
    glVertex3f(0,0,-b);
    glVertex3f(a,-a,0);
    glVertex3f(0,0,-b);
    glVertex3f(-a,-a,0);
    glVertex3f(0,0,-b);
    glVertex3f(-a,a,0);
    glVertex3f(0,0,-b);

    glEnd();
    glPopMatrix();
}

// NOTICE
// 这个是最后我们想要的效果;通过调用该函数,指定物体大小、线宽、位姿以及颜色等，就可以绘制出相应物体的线框模型
void drawOctahedronFrame2(double size, double line_width, Eigen::Isometry3d& Twc)
{
    std::vector<GLfloat> glTwc=eigen2glfloat(Twc);

    const double a=size;
    const double b=a*sqrt(2);

    glPushMatrix();
    glMultMatrixf((GLfloat*)glTwc.data());

    glLineWidth(line_width);
    // 颜色
    glColor3f(0.5f,0.8f,0.12f);
    // 绘制直线,所有的点相连组成一组线段,对于圆圈形状,推荐使用这种绘图方式,因为它比较快
    // 还有一个 GL_LINE_STRIP ,不过这个不是首尾相连
    glBegin(GL_LINE_LOOP);
    // 先绘制正方形
    glVertex3f(a,a,0);
    glVertex3f(a,-a,0);
    glVertex3f(-a,-a,0);
    glVertex3f(-a,a,0);
    glEnd();

    glBegin(GL_LINE_LOOP);
    // 继续绘制
    glVertex3f(0,0,b);
    glVertex3f(a,a,0);
    glVertex3f(0,0,-b);
    glVertex3f(-a,-a,0);
    glEnd();

    glBegin(GL_LINE_LOOP);
    // 继续绘制
    glVertex3f(0,0,b);
    glVertex3f(a,-a,0);
    glVertex3f(0,0,-b);
    glVertex3f(-a,a,0);
    glEnd();

    glPopMatrix();
}

void drawOctahedronTriangles(double size, double line_width, Eigen::Isometry3d& Twc)
{
    std::vector<GLfloat> glTwc=eigen2glfloat(Twc);

    const double a=size;
    const double b=a*sqrt(2);

    glPushMatrix();
    glMultMatrixf((GLfloat*)glTwc.data());

    glLineWidth(line_width);
    // 颜色
    glColor3f(0.1f,1.0f,0.32f);
    // 绘制三角面片
    glBegin(GL_TRIANGLES);
    // 先绘制正方形上方的四个三角片
    glVertex3f(a,a,0);
    glVertex3f(a,-a,0);
    glVertex3f(0,0,b);

    glVertex3f(a,a,0);
    glVertex3f(-a,a,0);
    glVertex3f(0,0,b);

    glVertex3f(-a,-a,0);
    glVertex3f(a,-a,0);
    glVertex3f(0,0,b);

    glVertex3f(-a,-a,0);
    glVertex3f(-a,a,0);
    glVertex3f(0,0,b);

    // 然后是正方形下方的四个三角片
    glVertex3f(a,a,0);
    glVertex3f(a,-a,0);
    glVertex3f(0,0,-b);

    glVertex3f(a,a,0);
    glVertex3f(-a,a,0);
    glVertex3f(0,0,-b);

    glVertex3f(-a,-a,0);
    glVertex3f(a,-a,0);
    glVertex3f(0,0,-b);

    glVertex3f(-a,-a,0);
    glVertex3f(-a,a,0);
    glVertex3f(0,0,-b);

    glEnd();

    // 为了可视化效果,再绘制线框(和drawOctahedronFrame2中操作相同)
    glColor3f(0.0f,0.0f,0.0f);
    glLineWidth(10*line_width);

    glBegin(GL_LINE_LOOP);
    // 先绘制正方形
    glVertex3f(a,a,0);
    glVertex3f(a,-a,0);
    glVertex3f(-a,-a,0);
    glVertex3f(-a,a,0);
    glEnd();

    glBegin(GL_LINE_LOOP);
    // 继续绘制
    glVertex3f(0,0,b);
    glVertex3f(a,a,0);
    glVertex3f(0,0,-b);
    glVertex3f(-a,-a,0);
    glEnd();

    glBegin(GL_LINE_LOOP);
    // 继续绘制
    glVertex3f(0,0,b);
    glVertex3f(a,-a,0);
    glVertex3f(0,0,-b);
    glVertex3f(-a,a,0);
    glEnd();

    glPopMatrix();

}

#endif // __DEMO_HPP__


#include <boost/random.hpp>
#include <iostream>
#include <ctime>
#include <cstdint>

int main()
{
  std::time_t now = std::time(0);
  boost::random::mt19937 gen{static_cast<std::uint32_t>(now)};
  std::cout << gen() << '\n';
}


/*
@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/integral.hpp>
using namespace boost::hana;

template <typename T>
struct test {
    static_assert(value(integral<T, 0>) == 0, "");
    static_assert(value(integral<T, 1>) == 1, "");
};

template struct test<int>;
template struct test<long>;

int main() { }


#ifndef TEST_UNIT_TORSTEN_PMX_ODE_TEST_FIXTURE
#define TEST_UNIT_TORSTEN_PMX_ODE_TEST_FIXTURE

#include <stan/math/rev/mat.hpp>
#include <gtest/gtest.h>
#include <boost/numeric/odeint.hpp>
#include <test/unit/math/torsten/test_util.hpp>
#include <test/unit/math/prim/arr/functor/harmonic_oscillator.hpp>
#include <test/unit/math/prim/arr/functor/lorenz.hpp>
#include <iostream>
#include <sstream>
#include <vector>
#include <string>

using torsten::dsolve::PMXCvodesSystem;
using torsten::dsolve::PMXCvodesFwdSystem;

/*
 * The problem is from chemical kinetics, from CVODES examples
 */
struct chemical_kinetics {
  template <typename T0, typename T1, typename T2>
  inline std::vector<typename stan::return_type<T1, T2>::type>
  operator()(const T0& t_in,
             const std::vector<T1>& y,
             const std::vector<T2>& theta, const std::vector<double>& x_r,
             const std::vector<int>& x_i, std::ostream* msgs) const {
    if (y.size() != 3)
      throw std::domain_error(
          "the ODE RHS function was called with inconsistent state");

    std::vector<typename stan::return_type<T1, T2>::type> rhs(3);
    rhs[0] = -theta[0] * y[0] + theta[1] * y[1] * y[2];
    rhs[1] =  theta[0] * y[0] - theta[1] * y[1] * y[2] - theta[2] * y[1] * y[1];
    rhs[2] =  theta[2] * y[1] * y[1];

    return rhs;
  }
};

struct TorstenOdeTest : public testing::Test {
  // for events generation
  const int nt;
  std::vector<double> time;
  std::vector<double> amt;
  std::vector<double> rate;
  std::vector<int> cmt;
  std::vector<int> evid;
  std::vector<double> ii;
  std::vector<int> addl;
  std::vector<int> ss;
  std::vector<std::vector<double> > pMatrix;
  std::vector<std::vector<double> > biovar;
  std::vector<std::vector<double> > tlag;

  // for ODE integrator
  double t0;
  std::vector<double> x_r;
  std::vector<int> x_i;
  double rtol;
  double atol;
  int max_num_steps;
  std::ostream* msgs;

  void SetUp() {
    // make sure memory's clean before starting each test
    stan::math::recover_memory();
  }
  TorstenOdeTest() :
    nt(54),
    time{0, 0.083, 0.167, 0.25, 0.5, 0.75, 1, 1.5, 2, 3, 4, 6, 8, 12,
      12.083, 12.167, 12.25, 12.5, 12.75, 13, 13.5, 14, 15, 16, 18, 20, 24, 36,
      48, 60, 72, 84, 96, 108, 120, 132, 144, 156, 168, 168.083, 168.167, 168.25,
      168.5, 168.75, 169, 169.5, 170, 171, 172, 174, 176, 180, 186, 192},
    amt(nt, 0),
    rate(nt, 0),
    cmt(nt, 2),
    evid(nt, 0),
    ii(nt, 0),
    addl(nt, 0),
    ss(nt, 0),
    pMatrix{ {7.4, 1.1, 28, 78, 68 } },
    biovar{ { 1, 1, 1 } },
    tlag{ { 0, 0, 0 } },
    t0(0.0),
    rtol             {1.E-10},
    atol             {1.E-10},
    max_num_steps    {100000},
    msgs             {nullptr} {
      amt[0]  = 80000;
      cmt[0]  = 1;
      evid[0] = 1;
      ii[0]   = 12;
      addl[0] = 14;
      SetUp();
  }

  template <typename Ode>
  void test_cvodes_system(Ode& ode,
                          const std::vector<double>& y0,
                          const std::vector<double>& theta,
                          const std::vector<double>& ts) {
    auto user_data = ode.to_user_data();
    Ode* odep = static_cast<Ode* >(user_data);

    EXPECT_EQ(odep -> n(), y0.size());
    EXPECT_EQ(odep -> n_par(), theta.size());
    auto y = odep -> nv_y();
    for (size_t i = 0; i < ode.n(); ++i) {
      EXPECT_EQ(NV_Ith_S(y, i), y0[i]);
    }

    N_Vector res = N_VNew_Serial(y0.size());
    EXPECT_EQ(torsten::dsolve::cvodes_rhs<Ode>()(ts.back(), y, res, user_data), 0);
    auto fval = ode.f()(ts.back(), y0, theta, x_r, x_i, msgs);
    for (size_t i = 0; i < y0.size(); ++i)
      EXPECT_EQ(NV_Ith_S(res, i), fval[i]);
  }
};

struct TorstenOdeTest_sho : public TorstenOdeTest {
  const harm_osc_ode_fun f;
  std::vector<double> ts;
  std::vector<double> theta;
  std::vector<double> y0;

  using F = harm_osc_ode_fun;

  TorstenOdeTest_sho() :
    f(),
    ts           {0.1, 0.2, 0.3, 10},
    theta        {0.15},
    y0           {1.0, 0.0}
  {}
};

struct TorstenOdeTest_chem : public TorstenOdeTest {
  const chemical_kinetics f;
  std::vector<double> ts;
  std::vector<double> theta;
  std::vector<double> y0;

  using F = chemical_kinetics;

  TorstenOdeTest_chem() :
    f(),
    ts          {0.4, 4.0, 40.0},
    theta       {0.04, 1.E4, 3.E7},
    y0          {1.0, 0.0, 0.0}
  {}
};

struct TorstenOdeTest_lorenz : public TorstenOdeTest {
  const lorenz_ode_fun f;
  std::vector<double> ts;
  std::vector<double> theta;
  std::vector<double> y0;

  using F = lorenz_ode_fun;

  TorstenOdeTest_lorenz() :
    f(),
    ts        {0.1, 1.0, 10.0},
    theta     {10.0, 28.0, 8.0/3.0},
    y0        {10.0, 1.0, 1.0}
  {}
};

struct TwoCptNeutModelODE {
  template <typename T0, typename T1, typename T2>
  inline std::vector<typename stan::return_type<T1, T2>::type>
  operator()(const T0& t_in,
             const std::vector<T1>& x,
             const std::vector<T2>& theta,
             const std::vector<double>& x_r,
             const std::vector<int>& x_i,
             std::ostream* msgs) const {
    using scalar_type = typename stan::return_type<T1, T2>::type;

    const T2& CL    = theta[0];
    const T2& Q     = theta[1];
    const T2& V1    = theta[2];
    const T2& V2    = theta[3];
    const T2& ka    = theta[4];
    const T2& mtt   = theta[5];
    const T2& circ0 = theta[6];
    const T2& gamma = theta[7];
    const T2& alpha = theta[8];

    T2 k10 = CL / V1;
    T2 k12 = Q / V1;
    T2 k21 = Q / V2;
    T2 ktr = 4 / mtt;

    std::vector<scalar_type> dxdt(8);
    dxdt[0] = -ka * x[0];
    dxdt[1] = ka * x[0] - (k10 + k12) * x[1] + k21 * x[2];
    dxdt[2] = k12 * x[1] - k21 * x[2];
    scalar_type conc = x[1]/V1;
    scalar_type EDrug = alpha * conc;

    // x[4], x[5], x[6], x[7] and x[8] are differences from circ0.
    scalar_type prol = x[3] + circ0;
    scalar_type transit1 = x[4] + circ0;
    scalar_type transit2 = x[5] + circ0;
    scalar_type transit3 = x[6] + circ0;
    scalar_type circ = stan::math::fmax(stan::math::machine_precision(), x[7] + circ0);

    dxdt[3] = ktr * prol * ((1 - EDrug) * (pow(circ0 / circ, gamma) - 1));
    dxdt[4] = ktr * (prol - transit1);
    dxdt[5] = ktr * (transit1 - transit2);
    dxdt[6] = ktr * (transit2 - transit3);
    dxdt[7] = ktr * (transit3 - circ);

    return dxdt;
  }
};

struct TorstenOdeTest_neutropenia : public TorstenOdeTest {
  const TwoCptNeutModelODE f;
  std::vector<double> ts;
  std::vector<double> theta;
  std::vector<double> y0;

  using F = TwoCptNeutModelODE;

  TorstenOdeTest_neutropenia() :
    f(),
    ts        {0.083, 0.167, 0.25, 0.5, 0.75, 1, 1.5, 2,3,4,6,8,12,24,36,48,60,72,200},
    theta     {10, 15, 35, 105, 2, 125, 5, 0.17, 2.0e-4},
    y0        {100.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}
  {}
};

#endif


#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <cv_bridge/cv_bridge.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <image_transport/image_transport.h>

#include <dynamic_reconfigure/server.h>
#include <pointcloud_to_rangeimage/PointCloudToRangeImageReconfigureConfig.h>

#include <pcl/point_types.h>
#include <pcl/range_image/range_image_spherical.h>
#include <velodyne_pcl/point_types.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <math.h>
#include <boost/thread/mutex.hpp>

#include "pointcloud_to_rangeimage/utils.h"
#include <pointcloud_to_rangeimage/RangeImage.h>
#include "pointcloud_to_rangeimage/range_image_expand.h"

namespace
{
  typedef pcl::PointXYZI PointType;
  typedef pcl::PointCloud<PointType> PointCloud;

  typedef pcl::RangeImage RI;
  typedef pcl::RangeImageWithoutInterpolation RIS;

  typedef pointcloud_to_rangeimage::PointCloudToRangeImageReconfigureConfig conf;
  typedef dynamic_reconfigure::Server<conf> RangeImageReconfServer;

  typedef image_transport::ImageTransport It;
  typedef image_transport::Subscriber Sub;
}

class PointCloudConverter
/*
Receive messages of type RangeImage from topic /msg_decoded, project the range, azimuth and intensity images
back to point cloud. The point cloud of type sensor_msgs::PointCloud2 is then published to the topic /pointcloud_out.
*/
{
private:
  bool _newmsg;
  bool _laser_frame;
  bool _init;

  // RangeImage frame
  pcl::RangeImage::CoordinateFrame _frame;

  // RangeImage resolution
  double _ang_res_x;
  double _azi_scale = (2 * static_cast<float>(M_PI)) / std::numeric_limits<ushort>::max();

  // Sensor min/max range
  float _min_range;
  float _max_range;

  double _vlp_rpm;
  double _firing_cycle;
  int _az_increments;
  std::vector<int> _az_shifts;
  std::vector<double> _azimuth_offsets;
  std::vector<double> _elevation_offsets;
  std::vector<int> _nans_row;
  std::vector<int> _nans_col;

  boost::mutex _mut;

  std_msgs::Header _header;
  ros::Time _send_time;

  cv_bridge::CvImagePtr _rangeImage;
  cv_bridge::CvImagePtr _intensityMap;
  cv_bridge::CvImagePtr _azimuthMap;

  PointCloud _pointcloud;
  boost::shared_ptr<RIS> rangeImageSph_;
  ros::NodeHandle nh_;
  ros::Publisher pub_;
  ros::Subscriber sub_;
  It it_;
  float delay_sum_=0;
  int frame_count_=0;
  boost::shared_ptr<RangeImageReconfServer> drsv_;

public:
  PointCloudConverter() : _newmsg(false),
                          _laser_frame(true),
                          _init(false),
                          _ang_res_x(600 * (1.0 / 60.0) * 360.0 * 0.000055296),
                          _min_range(0.4),
                          _max_range(200),
                          it_(nh_),
                          nh_("~")
  {
    rangeImageSph_ = boost::shared_ptr<RIS>(new RIS);
    drsv_.reset(new RangeImageReconfServer(ros::NodeHandle("rangeimage_to_pointcloud_dynreconf")));
    RangeImageReconfServer::CallbackType cb;
    cb = boost::bind(&PointCloudConverter::drcb, this, _1, _2);
    drsv_->setCallback(cb);

    // Get parameters from configuration file.
    while (!nh_.getParam("/point_cloud_to_rangeimage/vlp_rpm", _vlp_rpm))
    {
      ROS_WARN("Could not get Parameter 'vlp_rpm'! Retrying!");
    }
    ROS_INFO_STREAM("RPM set to: " << _vlp_rpm);

    while (!nh_.getParam("/point_cloud_to_rangeimage/firing_cycle", _firing_cycle))
    {
      ROS_WARN("Could not get Parameter 'firing_cycle'! Retrying!");
    }
    ROS_INFO_STREAM("Firing Cycle set to: " << _firing_cycle << " s");

    while (!nh_.getParam("/point_cloud_to_rangeimage/elevation_offsets", _elevation_offsets))
    {
      ROS_WARN("Could not get Parameter 'elevation_offsets'! Retrying!");
    }

    nh_.param("laser_frame", _laser_frame, _laser_frame);

    double min_range = static_cast<double>(_min_range);
    double max_range = static_cast<double>(_max_range);
    nh_.param("min_range", min_range, min_range);
    nh_.param("max_range", max_range, max_range);
    _min_range = static_cast<float>(min_range);
    _max_range = static_cast<float>(max_range);

    _ang_res_x = _vlp_rpm * (1.0 / 60.0) * 360.0 * _firing_cycle;
    _az_increments = (int)ceil(360.0f / _ang_res_x); //1809
    pub_ = nh_.advertise<sensor_msgs::PointCloud2>("pointcloud_out", 1);

    std::string transport = "raw";
    nh_.param("transport", transport, transport);
    if (transport != "raw" && transport != "compressedDepth")
    {
      ROS_WARN_STREAM("Transport " << transport
                                   << ".\nThe only transports supported are :\n\t - raw\n\t - compressedDepth.\n"
                                   << "Setting transport default 'raw'.");
      transport = "raw";
    }
    else
      ROS_INFO_STREAM("Transport " << transport);
    image_transport::TransportHints transportHint(transport);
    std::string image_in = "image_in";
    nh_.param("image_in", image_in, image_in);

    sub_ = nh_.subscribe("/msg_decoded", 1, &PointCloudConverter::callback, this);
    _frame = (_laser_frame) ? pcl::RangeImage::LASER_FRAME : pcl::RangeImage::CAMERA_FRAME;
  }

  ~PointCloudConverter()
  {
  }

  void callback(const pointcloud_to_rangeimage::RangeImageConstPtr &msg)
  {
    if (msg == NULL)
      return;

    boost::mutex::scoped_lock lock(_mut);

    // Copy images to OpenCV image pointer.
    try
    {
      _rangeImage = cv_bridge::toCvCopy(msg->RangeImage, msg->RangeImage.encoding);
      _intensityMap = cv_bridge::toCvCopy(msg->IntensityMap, msg->IntensityMap.encoding);
      _azimuthMap = cv_bridge::toCvCopy(msg->AzimuthMap, msg->AzimuthMap.encoding);
      _header = msg->header;
      _send_time = msg->send_time;
      _newmsg = true;
    }
    catch (cv_bridge::Exception &e)
    {
      ROS_WARN_STREAM(e.what());
    }

    // Get nan coordinates
    _nans_row.clear();
    _nans_col.clear();
    for (int i = 0; i < msg->NansRow.size(); i++)
    {
      _nans_row.push_back(static_cast<int>(msg->NansRow[i]));
      _nans_col.push_back(static_cast<int>(msg->NansCol[i]));
    }

//    cv::imwrite("/home/rosuser/catkin_ws/images/rangeimage_intensity_decoded.png", _intensityMap->image);
//    cv::imwrite("/home/rosuser/catkin_ws/images/rangeimage_range_decoded.png", _rangeImage->image);
//    cv::imwrite("/home/rosuser/catkin_ws/images/rangeimage_azimuth_decoded.png", _azimuthMap->image);
  }

  void convert()
  {
    // What the point if nobody cares ?
    if (pub_.getNumSubscribers() <= 0)
      return;
    if (_rangeImage == NULL)
      return;
    if (!_newmsg)
      return;
    _pointcloud.clear();

    float factor = 1.0f / (_max_range - _min_range);
    float offset = -_min_range;

    _mut.lock();

    int cols = _rangeImage->image.cols;
    int rows = _rangeImage->image.rows;
    rangeImageSph_->createEmpty(cols, rows, pcl::deg2rad(_ang_res_x), Eigen::Affine3f::Identity(), _frame);
    bool mask[32][1812]{false};
    // Mask nans as -1 in the images.
    for (int i = 0; i < _nans_row.size(); i++)
    {
      mask[_nans_row[i]][_nans_col[i]] = true;
    }

    // Decode ranges.
    int point_counter = 0;
    int uninf_counter = 0;
    for (int i = 0; i < cols; ++i)
    {
      for (int j = 0; j < rows; ++j)
      {
        ushort range_img = _rangeImage->image.at<ushort>(j, i);
        // Discard unobserved points.
        if (mask[j][i])
          continue;

        // Rescale range.
        float range = static_cast<float>(range_img) /
                      static_cast<float>(std::numeric_limits<ushort>::max());

        range = (range - offset * factor) / factor;
        pcl::PointWithRange &p = rangeImageSph_->getPointNoCheck(i, j);
        // Read intensity value.
        float &intensity = rangeImageSph_->intensities[j * cols + i];
        intensity = static_cast<float>(_intensityMap->image.at<uchar>(j, i));
        // Read azimuth angle.
        ushort azi_img = _azimuthMap->image.at<ushort>(j, i);
        float azi = static_cast<float>(azi_img) * _azi_scale - static_cast<float>(M_PI);
        float &azimuth = rangeImageSph_->azimuth[j * cols + i];
        azimuth = static_cast<float>(azi);
        p.range = range;
        point_counter++;
      }
    }

    // Reconstruct 3D point positions.
    rangeImageSph_->recalculate3DPointPositionsVelodyne(_elevation_offsets, pcl::deg2rad(_ang_res_x), cols, rows);

    // Reconstruct point cloud.
    for (int i = 0; i < rangeImageSph_->points.size(); ++i)
    {
      pcl::PointWithRange &pts = rangeImageSph_->points[i];
      // Discard unobserved points
      if (std::isinf(pts.range))
        continue;

      // PointType p(pts.x, pts.y, pts.z);
      PointType p;
      p.x = pts.x;
      p.y = pts.y;
      p.z = pts.z;
      p.intensity = rangeImageSph_->intensities[i];

      _pointcloud.push_back(p);
    }

    //clear points
    _init = false;

    sensor_msgs::PointCloud2 ros_pc;
    pcl::toROSMsg(_pointcloud, ros_pc);
    ros_pc.header = _header;
    delay_sum_ += ros::Time::now().toSec() - _send_time.toSec();
    frame_count_++;
    // Calculate delay of application.
    // std::cout << "Delay: " << ros::Time::now().toSec() - _send_time.toSec() << " seconds" << std::endl;
    // std::cout << "Average delay: " << delay_sum_/frame_count_ << " seconds" << std::endl;
    pub_.publish(ros_pc);
    _newmsg = false;
    _mut.unlock();
  }

private:
  void drcb(conf &config, uint32_t level)
  {
    _min_range = config.min_range;
    _max_range = config.max_range;
    _laser_frame = config.laser_frame;

    _frame = (_laser_frame) ? pcl::RangeImage::LASER_FRAME : pcl::RangeImage::CAMERA_FRAME;

    ROS_INFO_STREAM("ang_res_x " << _ang_res_x);
    ROS_INFO_STREAM("min_range " << _min_range);
    ROS_INFO_STREAM("max_range " << _max_range);

    if (_laser_frame)
      ROS_INFO_STREAM("Frame type : "
                      << "LASER");
    else
      ROS_INFO_STREAM("Frame type : "
                      << "CAMERA");

    _init = false;
  }
};

int main(int argc, char **argv)
{
  ros::init(argc, argv, "rangeimage_to_pointcloud");

  PointCloudConverter converter;

  ros::Rate rate(15);

  while (ros::ok())
  {
    converter.convert();

    ros::spinOnce();

    rate.sleep();
  }
}


/*
 * vectors.cpp
 * Copyright (C) 2018 CESAR SINCHIGUANO <cesarsinchiguano@hotmail.es>
 *
 * Distributed under terms of the BSD license.
 */
// vtest.cpp: simple program to demonstrate basic usage of Vector class

// #include "Vector.h"
#include <iostream>
#include<vector>
#include <array>
#include <utility>

#include <boost/algorithm/minmax.hpp>
#include <boost/tuple/tuple.hpp>

#include <boost/algorithm/minmax_element.hpp>

//g++ -std=c++14 -o temp sort.cpp

int main(int argc, char const *argv[])
{
  // /* code */
  // int i = 2;
  // int j = 1;
  //
  // boost::tuples::tuple<const int&, const int&> temp = boost::minmax(i, j);
  //
  // std::cout << temp.get<0>() << '\n';
  // std::cout << temp.get<1>() << '\n';

  typedef std::array<int, 4> ARRAY;
  ARRAY tmpARRAY={2, 3, 0, 1};

  auto k=tmpARRAY.begin();
  auto l=tmpARRAY.end();

  for (auto i=tmpARRAY.begin(); i!=tmpARRAY.end() ;i++)
  {
      std::cout << *i << '\n';
  }

  std::pair< std::array<int, 4> ::iterator,std::array<int, 4> ::iterator> tempITERATOR=
    boost::minmax_element(k,l);

  std::cout << *tempITERATOR.first << '\n';
  std::cout << *tempITERATOR.second << '\n';

  std::cout << "/* message */" << '\n';
  std::cout << "/* message */" << '\n';
  ////////////////////////////////////
  ///////////////VECTORS////////////
  ///////////////////////////////////
  std::vector<int> v={2, 3, 0, 1};

  std::vector<int>::iterator m=v.begin();
  std::vector<int>::iterator n=v.begin();

  for (auto i=v.begin(); i!=v.end() ;i++)
  {
      std::cout << *i << '\n';
  }

  std::pair< std::vector<int>::iterator,std::vector<int>::iterator> it=
    boost::minmax_element(m,n);

  std::cout << *it.first << '\n';
  std::cout << *it.second << '\n';

  return 0;
}


#include <boost/math/distributions/complement.hpp>


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_FACTORIZATIONS_CHOL_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_FACTORIZATIONS_CHOL_HPP_INCLUDED

#include <nt2/linalg/functions/lu.hpp>
#include <nt2/include/functions/qr.hpp>
#include <nt2/include/functions/is_gtz.hpp>
#include <nt2/include/functions/potrf.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/functions/triu.hpp>
#include <nt2/include/functions/tril.hpp>
#include <nt2/linalg/details/utility/f77_wrapper.hpp>
#include <nt2/linalg/details/utility/lapack_verify.hpp>
#include <nt2/linalg/options.hpp>
#include <nt2/core/container/dsl/as_terminal.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/core/container/colon/colon.hpp>
#include <boost/assert.hpp>
#include <boost/dispatch/attributes.hpp>

namespace nt2 { namespace ext
{
  //============================================================================
  //CHOL Scalar
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( chol_, tag::cpu_
                            , (A0)
                            , (scalar_<unspecified_<A0> >)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0) const
    {
      BOOST_ASSERT_MSG(is_gtz(a0), "Matrix must be positive definite");
      return  nt2::sqrt(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( chol_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                            )
  {
    typedef typename nt2::meta::as_real<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&) const
    {
      BOOST_ASSERT_MSG(is_gtz(a0), "Matrix must be positive definite");
      return nt2::sqrt(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( chol_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                              (unspecified_<A2>)
                            )
  {
    typedef typename nt2::meta::as_real<A0>::type result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A1&, const A2&) const
    {
      BOOST_ASSERT_MSG(is_gtz(a0), "Matrix must be positive definite");
      return nt2::sqrt(a0);
    }
  };
  //============================================================================
  //Cholesky factorization
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( chol_, tag::cpu_
                            , (A0)(N0)(A1)(N1)
                            , ((node_<A0, nt2::tag::chol_
                                    , N0, nt2::container::domain
                                      >
                              ))
                              ((node_<A1, nt2::tag::tie_
                                    , N1, nt2::container::domain
                                     >
                              ))
                            )
  {
    typedef void  result_type;
    typedef typename boost::proto::result_of::child_c<A0&,0>::value_type child0;
    typedef typename child0::value_type                                  type_t;
    typedef nt2::memory::container<tag::table_,  type_t, nt2::_2D>   o_semantic;

    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      eval(a0, a1, N0(), N1());
    }

    //==========================================================================
    /// INTERNAL ONLY - R = chol(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0 ,  A1& a1
              , boost::mpl::long_<1> const& , boost::mpl::long_<1> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      NT2_LAPACK_VERIFY(nt2::potrf(boost::proto::value(a),'U'));
      boost::proto::child_c<0>(a1) = nt2::triu(a);
    }

    //==========================================================================
    /// INTERNAL ONLY - L = chol(A,lower_/upper_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<1> const&
              ) const
    {
      eval( a0, a1, N0(), N1()
          , boost::proto::value(boost::proto::child_c<1>(a0))
          );
    }

    //==========================================================================
    /// INTERNAL ONLY - L = chol(A,lower_)
    BOOST_FORCEINLINE
    void eval( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<1> const&
              , nt2::policy<ext::lower_> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      NT2_LAPACK_VERIFY(nt2::potrf(boost::proto::value(a),'L'));
      boost::proto::child_c<0>(a1) = nt2::tril(a);
    }

    //==========================================================================
    /// INTERNAL ONLY - R = chol(A,upper_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<1> const&
              , nt2::policy<ext::upper_> const&
              ) const
    {
      eval(a0,a1,boost::mpl::long_<1>(),N1());
    }

    //==========================================================================
    /// INTERNAL ONLY - [R,P] = chol(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<1> const& , boost::mpl::long_<2> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      nt2_la_int info = nt2::potrf(boost::proto::value(a),'U');
      BOOST_ASSERT_MSG(info >= 0, "invalid parameter in potrf call");
      if (info == 0)
        boost::proto::child_c<0>(a1) = nt2::triu(a);
      else
        boost::proto::child_c<0>(a1) = nt2::triu(a(nt2::_(1, info-1), nt2::_(1, info-1)));
      boost::proto::child_c<1>(a1) = info;
    }

    //==========================================================================
    /// INTERNAL ONLY - [R,P] = chol(A,lower_/upper_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<2> const&
              ) const
    {
      eval( a0, a1, N0(), N1()
          , boost::proto::value(boost::proto::child_c<1>(a0))
          );
    }

    //==========================================================================
    /// INTERNAL ONLY - L = chol(A, raw_,lower_/upper_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<3> const& , boost::mpl::long_<2> const&
              ) const
    {
      eval( a0, a1, N0(), N1()
          , boost::proto::value(boost::proto::child_c<1>(a0))
          , boost::proto::value(boost::proto::child_c<2>(a0))
          );
    }

    //==========================================================================
    /// INTERNAL ONLY - [L,P] = chol(A,lower_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::lower_> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      nt2_la_int info = nt2::potrf(boost::proto::value(a),'L');
      BOOST_ASSERT_MSG(info >= 0, "invalid parameter in potrf call");
      if (info == 0)
        boost::proto::child_c<0>(a1) = nt2::tril(a);
      else
        boost::proto::child_c<0>(a1) = nt2::tril(a(nt2::_(1, info-1), nt2::_(1, info-1)));
      boost::proto::child_c<1>(a1) = info;
    }

    //==========================================================================
    /// INTERNAL ONLY - [R,P] = chol(A,upper_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::upper_> const&
              ) const
    {
      eval(a0,a1,
           boost::mpl::long_<1>(),N1());
    }

    //==========================================================================
    /// INTERNAL ONLY - [L,P] = chol(A,raw_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<2> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::raw_> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      nt2_la_int info = nt2::potrf(boost::proto::value(a),'U');
      BOOST_ASSERT_MSG(info >= 0, "invalid parameter in potrf call");
      assign_swap( boost::proto::child_c<0>(a1), a);
      boost::proto::child_c<1>(a1) = info;
    }

    //==========================================================================
    /// INTERNAL ONLY - [L,P] = chol(A,raw_,lower_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<3> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::raw_> const&
              , nt2::policy<ext::lower_> const&
              ) const
    {
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0)
                           , boost::proto::child_c<0>(a1));
      nt2_la_int info = nt2::potrf(boost::proto::value(a),'L');
      BOOST_ASSERT_MSG(info >= 0, "invalid parameter in potrf call");
      assign_swap( boost::proto::child_c<0>(a1), a);
      boost::proto::child_c<1>(a1) =  info;
    }

    //==========================================================================
    /// INTERNAL ONLY - [L,P] = chol(A,raw_,upper_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<3> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::raw_> const&
              , nt2::policy<ext::upper_> const&
              ) const
    {
      eval(a0, a1
          , boost::mpl::long_<2>(), N1(), nt2::policy<ext::raw_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [L,P] = chol(A,upper_,raw_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<3> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::upper_> const&
              , nt2::policy<ext::raw_> const&
              ) const
    {
      eval(a0, a1
          , boost::mpl::long_<2>(), N1(), nt2::policy<ext::raw_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - [L,P] = chol(A,lower_,raw_)
    BOOST_FORCEINLINE
    void eval ( A0& a0 , A1& a1
              , boost::mpl::long_<3> const& , boost::mpl::long_<2> const&
              , nt2::policy<ext::lower_> const&
              , nt2::policy<ext::raw_> const&
              ) const
    {
      eval(a0, a1
          , boost::mpl::long_<3>(),  boost::mpl::long_<2>()
          , nt2::policy<ext::raw_>(), nt2::policy<ext::lower_>());
    }

  };
} }

#endif


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

/* Author: Mark Moll */

#include "ompl/base/spaces/DubinsStateSpace.h"
#include "ompl/base/SpaceInformation.h"
#include "ompl/util/Exception.h"
#include <queue>
#include <boost/math/constants/constants.hpp>

using namespace ompl::base;

namespace
{
    const double twopi = 2. * boost::math::constants::pi<double>();
    const double DUBINS_EPS = 1e-6;
    const double DUBINS_ZERO = -1e-7;

    inline double mod2pi(double x)
    {
        if (x < 0 && x > DUBINS_ZERO)
            return 0;
        double xm = x - twopi * floor(x / twopi);
        if (twopi - xm < .5 * DUBINS_EPS) xm = 0.;
        return xm;
    }

    DubinsStateSpace::DubinsPath dubinsLSL(double d, double alpha, double beta)
    {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sa - sb));
        if (tmp >= DUBINS_ZERO)
        {
            double theta = atan2(cb - ca, d + sa - sb);
            double t = mod2pi(-alpha + theta);
            double p = sqrt(std::max(tmp, 0.));
            double q = mod2pi(beta - theta);
            assert(fabs(p * cos(alpha + t) - sa + sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha + t) + ca - cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha + t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[0], t, p, q);
        }
        return {};
    }

    DubinsStateSpace::DubinsPath dubinsRSR(double d, double alpha, double beta)
    {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = 2. + d * d - 2. * (ca * cb + sa * sb - d * (sb - sa));
        if (tmp >= DUBINS_ZERO)
        {
            double theta = atan2(ca - cb, d - sa + sb);
            double t = mod2pi(alpha - theta);
            double p = sqrt(std::max(tmp, 0.));
            double q = mod2pi(-beta + theta);
            assert(fabs(p * cos(alpha - t) + sa - sb - d) < 2* DUBINS_EPS);
            assert(fabs(p * sin(alpha - t) - ca + cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha - t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[1], t, p, q);
        }
        return {};
    }

    DubinsStateSpace::DubinsPath dubinsRSL(double d, double alpha, double beta)
    {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = d * d - 2. + 2. * (ca * cb + sa * sb - d * (sa + sb));
        if (tmp >= DUBINS_ZERO)
        {
            double p = sqrt(std::max(tmp, 0.));
            double theta = atan2(ca + cb, d - sa - sb) - atan2(2., p);
            double t = mod2pi(alpha - theta);
            double q = mod2pi(beta - theta);
            assert(fabs(p * cos(alpha - t) - 2. * sin(alpha - t) + sa + sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha - t) + 2. * cos(alpha - t) - ca - cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha - t + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[2], t, p, q);
        }
        return {};
    }

    DubinsStateSpace::DubinsPath dubinsLSR(double d, double alpha, double beta)
    {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = -2. + d * d + 2. * (ca * cb + sa * sb + d * (sa + sb));
        if (tmp >= DUBINS_ZERO)
        {
            double p = sqrt(std::max(tmp, 0.));
            double theta = atan2(-ca - cb, d + sa + sb) - atan2(-2., p);
            double t = mod2pi(-alpha + theta);
            double q = mod2pi(-beta + theta);
            assert(fabs(p * cos(alpha + t) + 2. * sin(alpha + t) - sa - sb - d) < 2 * DUBINS_EPS);
            assert(fabs(p * sin(alpha + t) - 2. * cos(alpha + t) + ca + cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha + t - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[3], t, p, q);
        }
        return {};
    }

    DubinsStateSpace::DubinsPath dubinsRLR(double d, double alpha, double beta)
    {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb + d * (sa - sb)));
        if (fabs(tmp) < 1.)
        {
            double p = twopi - acos(tmp);
            double theta = atan2(ca - cb, d - sa + sb);
            double t = mod2pi(alpha - theta + .5 * p);
            double q = mod2pi(alpha - beta - t + p);
            assert(fabs(2. * sin(alpha - t + p) - 2. * sin(alpha - t) - d + sa - sb) < 2 * DUBINS_EPS);
            assert(fabs(-2. * cos(alpha - t + p) + 2. * cos(alpha - t) - ca + cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha - t + p - q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[4], t, p, q);
        }
        return {};
    }

    DubinsStateSpace::DubinsPath dubinsLRL(double d, double alpha, double beta)
    {
        double ca = cos(alpha), sa = sin(alpha), cb = cos(beta), sb = sin(beta);
        double tmp = .125 * (6. - d * d + 2. * (ca * cb + sa * sb - d * (sa - sb)));
        if (fabs(tmp) < 1.)
        {
            double p = twopi - acos(tmp);
            double theta = atan2(-ca + cb, d + sa - sb);
            double t = mod2pi(-alpha + theta + .5 * p);
            double q = mod2pi(beta - alpha - t + p);
            assert(fabs(-2. * sin(alpha + t - p) + 2. * sin(alpha + t) - d - sa + sb) < 2 * DUBINS_EPS);
            assert(fabs(2. * cos(alpha + t - p) - 2. * cos(alpha + t) + ca - cb) < 2 * DUBINS_EPS);
            assert(mod2pi(alpha + t - p + q - beta + .5 * DUBINS_EPS) < DUBINS_EPS);
            return DubinsStateSpace::DubinsPath(DubinsStateSpace::dubinsPathType[5], t, p, q);
        }
        return {};
    }

    DubinsStateSpace::DubinsPath dubins(double d, double alpha, double beta)
    {
        if (d < DUBINS_EPS && fabs(alpha - beta) < DUBINS_EPS)
            return {DubinsStateSpace::dubinsPathType[0], 0, d, 0};

        DubinsStateSpace::DubinsPath path(dubinsLSL(d, alpha, beta)), tmp(dubinsRSR(d, alpha, beta));
        double len, minLength = path.length();

        if ((len = tmp.length()) < minLength)
        {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsRSL(d, alpha, beta);
        if ((len = tmp.length()) < minLength)
        {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsLSR(d, alpha, beta);
        if ((len = tmp.length()) < minLength)
        {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsRLR(d, alpha, beta);
        if ((len = tmp.length()) < minLength)
        {
            minLength = len;
            path = tmp;
        }
        tmp = dubinsLRL(d, alpha, beta);
        if ((len = tmp.length()) < minLength)
            path = tmp;
        return path;
    }
}

const ompl::base::DubinsStateSpace::DubinsPathSegmentType ompl::base::DubinsStateSpace::dubinsPathType[6][3] = {
    {DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_LEFT},
    {DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_RIGHT},
    {DUBINS_RIGHT, DUBINS_STRAIGHT, DUBINS_LEFT},
    {DUBINS_LEFT, DUBINS_STRAIGHT, DUBINS_RIGHT},
    {DUBINS_RIGHT, DUBINS_LEFT, DUBINS_RIGHT},
    {DUBINS_LEFT, DUBINS_RIGHT, DUBINS_LEFT}};

double ompl::base::DubinsStateSpace::distance(const State *state1, const State *state2) const
{
    if (isSymmetric_)
        return rho_ * std::min(dubins(state1, state2).length(), dubins(state2, state1).length());
    return rho_ * dubins(state1, state2).length();
}

void ompl::base::DubinsStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
{
    bool firstTime = true;
    DubinsPath path;
    interpolate(from, to, t, firstTime, path, state);
}

void ompl::base::DubinsStateSpace::interpolate(const State *from, const State *to, const double t, bool &firstTime,
                                               DubinsPath &path, State *state) const
{
    if (firstTime)
    {
        if (t >= 1.)
        {
            if (to != state)
                copyState(state, to);
            return;
        }
        if (t <= 0.)
        {
            if (from != state)
                copyState(state, from);
            return;
        }

        path = dubins(from, to);
        if (isSymmetric_)
        {
            DubinsPath path2(dubins(to, from));
            if (path2.length() < path.length())
            {
                path2.reverse_ = true;
                path = path2;
            }
        }
        firstTime = false;
    }
    interpolate(from, path, t, state);
}

void ompl::base::DubinsStateSpace::interpolate(const State *from, const DubinsPath &path, double t, State *state) const
{
    auto *s = allocState()->as<StateType>();
    double seg = t * path.length(), phi, v;

    s->setXY(0., 0.);
    s->setYaw(from->as<StateType>()->getYaw());
    if (!path.reverse_)
    {
        for (unsigned int i = 0; i < 3 && seg > 0; ++i)
        {
            v = std::min(seg, path.length_[i]);
            phi = s->getYaw();
            seg -= v;
            switch (path.type_[i])
            {
                case DUBINS_LEFT:
                    s->setXY(s->getX() + sin(phi + v) - sin(phi), s->getY() - cos(phi + v) + cos(phi));
                    s->setYaw(phi + v);
                    break;
                case DUBINS_RIGHT:
                    s->setXY(s->getX() - sin(phi - v) + sin(phi), s->getY() + cos(phi - v) - cos(phi));
                    s->setYaw(phi - v);
                    break;
                case DUBINS_STRAIGHT:
                    s->setXY(s->getX() + v * cos(phi), s->getY() + v * sin(phi));
                    break;
            }
        }
    }
    else
    {
        for (unsigned int i = 0; i < 3 && seg > 0; ++i)
        {
            v = std::min(seg, path.length_[2 - i]);
            phi = s->getYaw();
            seg -= v;
            switch (path.type_[2 - i])
            {
                case DUBINS_LEFT:
                    s->setXY(s->getX() + sin(phi - v) - sin(phi), s->getY() - cos(phi - v) + cos(phi));
                    s->setYaw(phi - v);
                    break;
                case DUBINS_RIGHT:
                    s->setXY(s->getX() - sin(phi + v) + sin(phi), s->getY() + cos(phi + v) - cos(phi));
                    s->setYaw(phi + v);
                    break;
                case DUBINS_STRAIGHT:
                    s->setXY(s->getX() - v * cos(phi), s->getY() - v * sin(phi));
                    break;
            }
        }
    }
    state->as<StateType>()->setX(s->getX() * rho_ + from->as<StateType>()->getX());
    state->as<StateType>()->setY(s->getY() * rho_ + from->as<StateType>()->getY());
    getSubspace(1)->enforceBounds(s->as<SO2StateSpace::StateType>(1));
    state->as<StateType>()->setYaw(s->getYaw());
    freeState(s);
}

ompl::base::DubinsStateSpace::DubinsPath ompl::base::DubinsStateSpace::dubins(const State *state1,
                                                                              const State *state2) const
{
    const auto *s1 = static_cast<const StateType *>(state1);
    const auto *s2 = static_cast<const StateType *>(state2);
    double x1 = s1->getX(), y1 = s1->getY(), th1 = s1->getYaw();
    double x2 = s2->getX(), y2 = s2->getY(), th2 = s2->getYaw();
    double dx = x2 - x1, dy = y2 - y1, d = sqrt(dx * dx + dy * dy) / rho_, th = atan2(dy, dx);
    double alpha = mod2pi(th1 - th), beta = mod2pi(th2 - th);
    return ::dubins(d, alpha, beta);
}

void ompl::base::DubinsMotionValidator::defaultSettings()
{
    stateSpace_ = dynamic_cast<DubinsStateSpace *>(si_->getStateSpace().get());
    if (stateSpace_ == nullptr)
        throw Exception("No state space for motion validator");
}

bool ompl::base::DubinsMotionValidator::checkMotion(const State *s1, const State *s2,
                                                    std::pair<State *, double> &lastValid) const
{
    /* assume motion starts in a valid configuration so s1 is valid */

    bool result = true, firstTime = true;
    DubinsStateSpace::DubinsPath path;
    int nd = stateSpace_->validSegmentCount(s1, s2);

    if (nd > 1)
    {
        /* temporary storage for the checked state */
        State *test = si_->allocState();

        for (int j = 1; j < nd; ++j)
        {
            stateSpace_->interpolate(s1, s2, (double)j / (double)nd, firstTime, path, test);
            if (!si_->isValid(test))
            {
                lastValid.second = (double)(j - 1) / (double)nd;
                if (lastValid.first != nullptr)
                    stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
                result = false;
                break;
            }
        }
        si_->freeState(test);
    }

    if (result)
        if (!si_->isValid(s2))
        {
            lastValid.second = (double)(nd - 1) / (double)nd;
            if (lastValid.first != nullptr)
                stateSpace_->interpolate(s1, s2, lastValid.second, firstTime, path, lastValid.first);
            result = false;
        }

    if (result)
        valid_++;
    else
        invalid_++;

    return result;
}

bool ompl::base::DubinsMotionValidator::checkMotion(const State *s1, const State *s2) const
{
    /* assume motion starts in a valid configuration so s1 is valid */
    if (!si_->isValid(s2))
        return false;

    bool result = true, firstTime = true;
    DubinsStateSpace::DubinsPath path;
    int nd = stateSpace_->validSegmentCount(s1, s2);

    /* initialize the queue of test positions */
    std::queue<std::pair<int, int>> pos;
    if (nd >= 2)
    {
        pos.emplace(1, nd - 1);

        /* temporary storage for the checked state */
        State *test = si_->allocState();

        /* repeatedly subdivide the path segment in the middle (and check the middle) */
        while (!pos.empty())
        {
            std::pair<int, int> x = pos.front();

            int mid = (x.first + x.second) / 2;
            stateSpace_->interpolate(s1, s2, (double)mid / (double)nd, firstTime, path, test);

            if (!si_->isValid(test))
            {
                result = false;
                break;
            }

            pos.pop();

            if (x.first < mid)
                pos.emplace(x.first, mid - 1);
            if (x.second > mid)
                pos.emplace(mid + 1, x.second);
        }

        si_->freeState(test);
    }

    if (result)
        valid_++;
    else
        invalid_++;

    return result;
}