// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018-2021 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include <Eigen/Dense>
#include <limits>

#include "open3d/camera/PinholeCameraIntrinsic.h"
#include "open3d/geometry/Image.h"
#include "open3d/geometry/PointCloud.h"
#include "open3d/geometry/RGBDImage.h"
#include "open3d/geometry/VoxelGrid.h"
#include "open3d/utility/Logging.h"

namespace open3d {

namespace {
using namespace geometry;

int CountValidDepthPixels(const Image &depth, int stride) {
    int num_valid_pixels = 0;
    for (int i = 0; i < depth.height_; i += stride) {
        for (int j = 0; j < depth.width_; j += stride) {
            const float *p = depth.PointerAt<float>(j, i);
            if (*p > 0) num_valid_pixels += 1;
        }
    }
    return num_valid_pixels;
}

std::shared_ptr<PointCloud> CreatePointCloudFromFloatDepthImage(
        const Image &depth,
        const camera::PinholeCameraIntrinsic &intrinsic,
        const Eigen::Matrix4d &extrinsic,
        int stride,
        bool project_valid_depth_only) {
    auto pointcloud = std::make_shared<PointCloud>();
    Eigen::Matrix4d camera_pose = extrinsic.inverse();
    auto focal_length = intrinsic.GetFocalLength();
    auto principal_point = intrinsic.GetPrincipalPoint();
    int num_valid_pixels;
    if (!project_valid_depth_only) {
        num_valid_pixels =
                int(depth.height_ / stride) * int(depth.width_ / stride);
    } else {
        num_valid_pixels = CountValidDepthPixels(depth, stride);
    }
    pointcloud->points_.resize(num_valid_pixels);
    int cnt = 0;
    for (int i = 0; i < depth.height_; i += stride) {
        for (int j = 0; j < depth.width_; j += stride) {
            const float *p = depth.PointerAt<float>(j, i);
            if (*p > 0) {
                double z = (double)(*p);
                double x = (j - principal_point.first) * z / focal_length.first;
                double y =
                        (i - principal_point.second) * z / focal_length.second;
                Eigen::Vector4d point =
                        camera_pose * Eigen::Vector4d(x, y, z, 1.0);
                pointcloud->points_[cnt++] = point.block<3, 1>(0, 0);
            } else if (!project_valid_depth_only) {
                double z = std::numeric_limits<float>::quiet_NaN();
                double x = std::numeric_limits<float>::quiet_NaN();
                double y = std::numeric_limits<float>::quiet_NaN();
                pointcloud->points_[cnt++] = Eigen::Vector3d(x, y, z);
            }
        }
    }
    return pointcloud;
}

template <typename TC, int NC>
std::shared_ptr<PointCloud> CreatePointCloudFromRGBDImageT(
        const RGBDImage &image,
        const camera::PinholeCameraIntrinsic &intrinsic,
        const Eigen::Matrix4d &extrinsic,
        bool project_valid_depth_only) {
    auto pointcloud = std::make_shared<PointCloud>();
    Eigen::Matrix4d camera_pose = extrinsic.inverse();
    auto focal_length = intrinsic.GetFocalLength();
    auto principal_point = intrinsic.GetPrincipalPoint();
    double scale = (sizeof(TC) == 1) ? 255.0 : 1.0;
    int num_valid_pixels;
    if (!project_valid_depth_only) {
        num_valid_pixels = image.depth_.height_ * image.depth_.width_;
    } else {
        num_valid_pixels = CountValidDepthPixels(image.depth_, 1);
    }
    pointcloud->points_.resize(num_valid_pixels);
    pointcloud->colors_.resize(num_valid_pixels);
    int cnt = 0;
    for (int i = 0; i < image.depth_.height_; i++) {
        float *p = (float *)(image.depth_.data_.data() +
                             i * image.depth_.BytesPerLine());
        TC *pc = (TC *)(image.color_.data_.data() +
                        i * image.color_.BytesPerLine());
        for (int j = 0; j < image.depth_.width_; j++, p++, pc += NC) {
            if (*p > 0) {
                double z = (double)(*p);
                double x = (j - principal_point.first) * z / focal_length.first;
                double y =
                        (i - principal_point.second) * z / focal_length.second;
                Eigen::Vector4d point =
                        camera_pose * Eigen::Vector4d(x, y, z, 1.0);
                pointcloud->points_[cnt] = point.block<3, 1>(0, 0);
                pointcloud->colors_[cnt++] =
                        Eigen::Vector3d(pc[0], pc[(NC - 1) / 2], pc[NC - 1]) /
                        scale;
            } else if (!project_valid_depth_only) {
                double z = std::numeric_limits<float>::quiet_NaN();
                double x = std::numeric_limits<float>::quiet_NaN();
                double y = std::numeric_limits<float>::quiet_NaN();
                pointcloud->points_[cnt] = Eigen::Vector3d(x, y, z);
                pointcloud->colors_[cnt++] =
                        Eigen::Vector3d(std::numeric_limits<TC>::quiet_NaN(),
                                        std::numeric_limits<TC>::quiet_NaN(),
                                        std::numeric_limits<TC>::quiet_NaN());
            }
        }
    }
    return pointcloud;
}

}  // unnamed namespace

namespace geometry {
std::shared_ptr<PointCloud> PointCloud::CreateFromDepthImage(
        const Image &depth,
        const camera::PinholeCameraIntrinsic &intrinsic,
        const Eigen::Matrix4d &extrinsic /* = Eigen::Matrix4d::Identity()*/,
        double depth_scale /* = 1000.0*/,
        double depth_trunc /* = 1000.0*/,
        int stride /* = 1*/,
        bool project_valid_depth_only) {
    if (depth.num_of_channels_ == 1) {
        if (depth.bytes_per_channel_ == 2) {
            auto float_depth =
                    depth.ConvertDepthToFloatImage(depth_scale, depth_trunc);
            return CreatePointCloudFromFloatDepthImage(
                    *float_depth, intrinsic, extrinsic, stride,
                    project_valid_depth_only);
        } else if (depth.bytes_per_channel_ == 4) {
            return CreatePointCloudFromFloatDepthImage(
                    depth, intrinsic, extrinsic, stride,
                    project_valid_depth_only);
        }
    }
    utility::LogError(
            "[CreatePointCloudFromDepthImage] Unsupported image format.");
    return std::make_shared<PointCloud>();
}

std::shared_ptr<PointCloud> PointCloud::CreateFromRGBDImage(
        const RGBDImage &image,
        const camera::PinholeCameraIntrinsic &intrinsic,
        const Eigen::Matrix4d &extrinsic /* = Eigen::Matrix4d::Identity()*/,
        bool project_valid_depth_only) {
    if (image.depth_.num_of_channels_ == 1 &&
        image.depth_.bytes_per_channel_ == 4) {
        if (image.color_.bytes_per_channel_ == 1 &&
            image.color_.num_of_channels_ == 3) {
            return CreatePointCloudFromRGBDImageT<uint8_t, 3>(
                    image, intrinsic, extrinsic, project_valid_depth_only);
        } else if (image.color_.bytes_per_channel_ == 1 &&
                   image.color_.num_of_channels_ == 4) {
            return CreatePointCloudFromRGBDImageT<uint8_t, 4>(
                    image, intrinsic, extrinsic, project_valid_depth_only);
        } else if (image.color_.bytes_per_channel_ == 4 &&
                   image.color_.num_of_channels_ == 1) {
            return CreatePointCloudFromRGBDImageT<float, 1>(
                    image, intrinsic, extrinsic, project_valid_depth_only);
        }
    }
    utility::LogError(
            "[CreatePointCloudFromRGBDImage] Unsupported image format.");
    return std::make_shared<PointCloud>();
}

std::shared_ptr<PointCloud> PointCloud::CreateFromVoxelGrid(
        const VoxelGrid &voxel_grid) {
    auto output = std::make_shared<PointCloud>();
    output->points_.resize(voxel_grid.voxels_.size());
    bool has_colors = voxel_grid.HasColors();
    if (has_colors) {
        output->colors_.resize(voxel_grid.voxels_.size());
    }
    size_t vidx = 0;
    for (auto &it : voxel_grid.voxels_) {
        const geometry::Voxel voxel = it.second;
        output->points_[vidx] =
                voxel_grid.GetVoxelCenterCoordinate(voxel.grid_index_);
        if (has_colors) {
            output->colors_[vidx] = voxel.color_;
        }
        vidx++;
    }
    return output;
}

}  // namespace geometry
}  // namespace open3d


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   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 BOOST_SIMD_CONSTANT_CONSTANTS_NBMANTISSABITS_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_NBMANTISSABITS_HPP_INCLUDED

#include <boost/simd/include/functor.hpp>
#include <boost/simd/meta/int_c.hpp>
#include <boost/simd/constant/hierarchy.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
   /*!
     @brief Nbmantissabits generic tag

     Represents the Nbmantissabits constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct Nbmantissabits : ext::pure_constant_<Nbmantissabits>
    {
      typedef double default_type;
      typedef ext::pure_constant_<Nbmantissabits> parent;

      /// INTERNAL ONLY
      template<class Target, class Dummy=void>
      struct  apply : meta::int_c < typename Target::type
                                  , sizeof(typename Target::type)*CHAR_BIT
                                  >
      {};
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_Nbmantissabits( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Nbmantissabits::apply<boost::dispatch::meta::single_<T>,Dummy>
          : meta::int_c<boost::simd::int32_t,23> {};

    /// INTERNAL ONLY
    template<class T, class Dummy>
    struct  Nbmantissabits::apply<boost::dispatch::meta::double_<T>,Dummy>
          : meta::int_c<boost::simd::int64_t,52> {};
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Nbmantissabits, Site> dispatching_Nbmantissabits(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Nbmantissabits, Site>();
   }
   template<class... Args>
   struct impl_Nbmantissabits;
  }
  /*!
    Constant Nbmantissabits, The number of mantissa bits of a floating point number.

    @par Semantic:

    @code
    T r = Nbmantissabits<T>();
    @endcode

    is similar to:

    @code
    if T is integral
      r = sizeof(T)*8
    else if T is double
      r =  52;
    else if is float
      r =  23;
    @endcode
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(boost::simd::tag::Nbmantissabits, Nbmantissabits)
} }

#include <boost/simd/constant/common.hpp>

#endif


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    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 Delft University of Technology 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 HOLDER 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.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      110208    D. Dirkx          First version of file.
 *      110210    L. Adulkadir      Code check.
 *      110211    K. Kumar          Corrected Doxygen errors; corrected layout errors; corrected
 *                                  double precision; updated function-naming.
 *      120605    J. Vandamme       Boostified unit test.
 *      120912    K. Kumar          Removed erroneous using-statements; added missing <cmath>
 *                                  include.
 *
 *    References
 *      Anderson Jr., J.D. , Fundamentals of Aerodynamics, 3rd edition, McGraw Hill, 2001.
 *      Anderson Jr. , J.D, Hypersonic and High-Temperature Gas Dynamics, 2nd edition,
 *          AIAA Education Series, 2006.
 *
 *    Notes
 *
 */

#define BOOST_TEST_MAIN

#include <cmath>

#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/unit_test.hpp>

#include "Tudat/Mathematics/BasicMathematics/mathematicalConstants.h"

#include "Tudat/Astrodynamics/Aerodynamics/aerodynamics.h"

namespace tudat
{
namespace unit_tests
{

BOOST_AUTO_TEST_SUITE( test_aerodynamics_namespace )

//! Test aerodynamic namespace pressure functions.
BOOST_AUTO_TEST_CASE( testAerodynamicNamespacePressureFunctions )
{
    using namespace tudat;
    using namespace aerodynamics;
    using mathematical_constants::PI;

    // Set default test conditions.
    const double machNumber_ = 12.0;
    const double ratioOfSpecificHeats_ = 1.4;

    // Test local to static pressure ratio.
    const double localToStaticPressureRatio_ = computeLocalToStaticPressureRatio(
           machNumber_, ratioOfSpecificHeats_ );

    const double expectedLocalToStaticPressureRatio_ = 1.0 / 0.1445e6;
    const double toleranceRatioCoefficient_ = 0.1445e6 * 1.0e-8 * 100.0
            / expectedLocalToStaticPressureRatio_;

    // Check if computed local-to-static pressure ratio matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( localToStaticPressureRatio_,
                                expectedLocalToStaticPressureRatio_,
                                toleranceRatioCoefficient_ );

    // Test stagnation pressure coefficient.
    const double stagnationPressureCoefficient_
            = computeStagnationPressure( machNumber_, ratioOfSpecificHeats_ );

    const double expectedStagnationPressureCoefficient_ = 1.83402;
    const double toleranceStagnationPressureCoefficient_ = 1.0e-5 * 100.0
            / expectedStagnationPressureCoefficient_;

    // Check if computed stagnation pressure coefficient matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( stagnationPressureCoefficient_,
                                expectedStagnationPressureCoefficient_,
                                toleranceStagnationPressureCoefficient_ );

    // Test modified Newtonian pressure coefficient.
    const double newtonianPressureCoefficient_ = computeModifiedNewtonianPressureCoefficient(
                PI / 2.0, stagnationPressureCoefficient_ );

    const double expectedNewtonianPressureCoefficient_ = stagnationPressureCoefficient_;
    const double toleranceNewtonianPressureCoefficient_ = 1.0e-15 * 100.0
            / expectedNewtonianPressureCoefficient_;

    // Check if computed Newtonian pressure coefficient matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( newtonianPressureCoefficient_,
                                expectedNewtonianPressureCoefficient_,
                                toleranceNewtonianPressureCoefficient_ );

    // Test empirical tangent cone pressure coefficient.
    const double empiricalTangentConePressureCoefficient_
            = computeEmpiricalTangentConePressureCoefficient( PI / 2.0, machNumber_ );

    const double expectedEmpiricalTangentConePressureCoefficient_ = 2.08961;
    const double toleranceEmpiricalTangentConePressureCoefficient_ = 1.0e-5 * 100.0
            / expectedEmpiricalTangentConePressureCoefficient_;

    // Check if computed empirical tangent cone pressure coefficient matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( empiricalTangentConePressureCoefficient_,
                                expectedEmpiricalTangentConePressureCoefficient_,
                                toleranceEmpiricalTangentConePressureCoefficient_ );

    // Test high Mach base pressure coefficient.
    const double highMachBasePressure_ = computeHighMachBasePressure( machNumber_ );

    const double expectedHighMachBasePressure_ = -1.0 / ( machNumber_ * machNumber_ );
    const double toleranceHighMachBasePressure_ = std::fabs( 1.0e-15 * 100.0 / expectedHighMachBasePressure_ );

    // Check if computed high Mach base pressure coefficient matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( highMachBasePressure_,
                                expectedHighMachBasePressure_,
                                toleranceHighMachBasePressure_ );

    // Test empirical tangent wedge pressure coefficient.
    const double empiricalTangentWedgePressureCoefficient_
            = computeEmpiricalTangentWedgePressureCoefficient( PI / 2.0, machNumber_ );

    const double expectedEmpiricalTangentWedgePressureCoefficient_ = 2.38867;
    const double toleranceEmpiricalTangentWedgePressureCoefficient_ = 1.0e-5 * 100.0
            / expectedEmpiricalTangentWedgePressureCoefficient_;

    // Check if computed tangent wedge pressure coefficient matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( empiricalTangentWedgePressureCoefficient_,
                                expectedEmpiricalTangentWedgePressureCoefficient_,
                                toleranceEmpiricalTangentWedgePressureCoefficient_ );

    // Test free-stream Prandtl-Meyer function.
    const double freestreamPrandtlMeyerFunction_ = computePrandtlMeyerFunction(
           machNumber_, ratioOfSpecificHeats_ );

    const double expectedFreeStreamPrandtlMeyerFunction_ = 106.9 * PI / 180.0;
    const double toleranceFreeStreamPrandtlMeyerFunction_ = 1.0e-3 * 100.0
            / expectedFreeStreamPrandtlMeyerFunction_;

    // Check if computed free-stream Prandtl-Meyer function matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( freestreamPrandtlMeyerFunction_,
                                expectedFreeStreamPrandtlMeyerFunction_,
                                toleranceFreeStreamPrandtlMeyerFunction_ );

    // Test vacuum pressure coefficient function.
    const double vacuumPressureCoefficient_ = computeVacuumPressureCoefficient(
           machNumber_, ratioOfSpecificHeats_ );

    const double expectedVacuumPressureCoefficient_ = -2.0 / ( ratioOfSpecificHeats_
                                                               * machNumber_ * machNumber_ );
    const double toleranceVacuumPressureCoefficient_ = std::fabs( 1.0e-15 * 100.0
            / expectedVacuumPressureCoefficient_ );

    // Check if computed vacuum pressure coefficient matches expected value.
    BOOST_CHECK_CLOSE_FRACTION( vacuumPressureCoefficient_,
                                expectedVacuumPressureCoefficient_,
                                toleranceVacuumPressureCoefficient_ );
}

//! Test pressure coefficients.
BOOST_AUTO_TEST_CASE( testPressureCoefficients )
{
    using std::sin;
    using namespace tudat;
    using namespace aerodynamics;
    using mathematical_constants::PI;

    // Set default conditions.
    const double machNumber_ = 12.0;
    const double ratioOfSpecificHeats_ = 1.4;

    // Iterate over 10 angles and test pressure coefficients.
    for ( int i = 0; i < 10; i++ )
    {
         // Set angle.
         const double angle_ = static_cast< double > ( i ) * PI / 10.0;

         // Compute and compare Newtonian pressure coefficient.
         const double newtonianPressureCoefficient_
                 = computeNewtonianPressureCoefficient( angle_ );

         const double expectedNewtonianPressureCoefficient_ =  2.0 * sin( angle_ ) * sin( angle_ );
         const double toleranceNewtonianPressureCoefficient_ = 1.0e-15 * 100.0
                 / expectedNewtonianPressureCoefficient_;

         // Check if computed Newtonian pressure coefficient matches expected value.
         BOOST_CHECK_CLOSE_FRACTION( newtonianPressureCoefficient_,
                                     expectedNewtonianPressureCoefficient_,
                                     toleranceNewtonianPressureCoefficient_ );

         // Compute Prandtl-Meyer pressure coefficient and test if it is not
         // lower than vacuum pressure coefficient.
         const double freestreamPrandtlMeyerFunction_ = computePrandtlMeyerFunction(
                     machNumber_, ratioOfSpecificHeats_ );

         const double prandtlMeyerPressureCoefficient_ =
                 computePrandtlMeyerFreestreamPressureCoefficient(
                     -1.0 * angle_, machNumber_, ratioOfSpecificHeats_,
                     freestreamPrandtlMeyerFunction_ );

         const double vacuumPressureCoefficient_ = computeVacuumPressureCoefficient(
                     machNumber_, ratioOfSpecificHeats_ );

         bool isPrandtlMeyerPressureCoefficient = false;

         if ( vacuumPressureCoefficient_ < prandtlMeyerPressureCoefficient_ + 1.0e-15 )
         {
             isPrandtlMeyerPressureCoefficient = true;
         }

         // Check if Prandt-Meyer pressure coefficient is lower than vacuum pressure coefficient.
         BOOST_CHECK( isPrandtlMeyerPressureCoefficient );

         // Test shock pressure ratio.
         const double shockPressureRatio_ = computeShockPressureRatio( machNumber_,
                                                                       ratioOfSpecificHeats_ );

         const double expectedShockPressureRatio_ = 167.8;
         const double toleranceShockPressureRatio_ = 0.1 * 100.0 / expectedShockPressureRatio_;

         // Check if computed shock pressure ratio matches expected value.
         BOOST_CHECK_CLOSE_FRACTION( shockPressureRatio_,
                                     expectedShockPressureRatio_,
                                     toleranceShockPressureRatio_ );

         // Test shock density ratio.
         const double shockDensityRatio_ = computeShockDensityRatio( machNumber_,
                                                                     ratioOfSpecificHeats_ );

         const double expectedShockDensityRatio_ = 5.799;
         const double toleranceShockDensityRatio_ = 0.001 * 100 / expectedShockDensityRatio_;

         // Check if computed shock density ratio matches expected value.
         BOOST_CHECK_CLOSE_FRACTION( shockDensityRatio_,
                                     expectedShockDensityRatio_,
                                     toleranceShockDensityRatio_ );

         // Test shock temperature ratio.
         const double shockTemperatureRatio_ = computeShockTemperatureRatio(
                     machNumber_, ratioOfSpecificHeats_ );

         const double expectedShockTemperatureRatio_ = 28.94;
         const double toleranceShockTemperatureRatio_ = 0.01 * 100.0
                 / expectedShockTemperatureRatio_;

         // Check if shock temperature ratio matches expected value.
         BOOST_CHECK_CLOSE_FRACTION( shockTemperatureRatio_,
                                     expectedShockTemperatureRatio_,
                                     toleranceShockTemperatureRatio_ );

         // Test shock total pressure ratio.
         const double shockTotalPressureRatio_ = computeShockTotalPressureRatio(
                     machNumber_, ratioOfSpecificHeats_, 287.058 );

         const double expectedShockTotalPressureRatio_ = 0.001287;
         const double toleranceShockTotalPressureRatio_ = 1.0e-6 *100.0
                 / expectedShockTotalPressureRatio_;

         // Check if shock total pressure ratio matches expected value.
         BOOST_CHECK_CLOSE_FRACTION( shockTotalPressureRatio_,
                                     expectedShockTotalPressureRatio_,
                                     toleranceShockTotalPressureRatio_ );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


// Copyright 2009-2014 Neil Groves.
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Copyright 2006 Thorsten Ottosen.
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Copyright 2004 Eric Niebler.
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
//   Contains range-based versions of the numeric std algorithms
//
#if defined(_MSC_VER)
    #pragma once
#endif

#ifndef BOOST_RANGE_NUMERIC_HPP
#define BOOST_RANGE_NUMERIC_HPP

#include <boost/config.hpp>
#include <boost/assert.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/category.hpp>
#include <boost/range/concepts.hpp>
#include <boost/range/distance.hpp>
#include <boost/range/size.hpp>
#include <numeric>

namespace boost
{
    template<class SinglePassRange, class Value>
    inline Value accumulate(const SinglePassRange& rng, Value init)
    {
        BOOST_RANGE_CONCEPT_ASSERT((
            SinglePassRangeConcept<const SinglePassRange>));

        return std::accumulate(boost::begin(rng), boost::end(rng), init);
    }

    template<class SinglePassRange, class Value, class BinaryOperation>
    inline Value accumulate(const SinglePassRange& rng, Value init,
                            BinaryOperation op)
    {
        BOOST_RANGE_CONCEPT_ASSERT((
                SinglePassRangeConcept<const SinglePassRange> ));

        return std::accumulate(boost::begin(rng), boost::end(rng), init, op);
    }

    namespace range_detail
    {
        template<class SinglePassRange1, class SinglePassRange2>
        inline bool inner_product_precondition(
            const SinglePassRange1&,
            const SinglePassRange2&,
            std::input_iterator_tag,
            std::input_iterator_tag)
        {
            return true;
        }

        template<class SinglePassRange1, class SinglePassRange2>
        inline bool inner_product_precondition(
            const SinglePassRange1& rng1,
            const SinglePassRange2& rng2,
            std::forward_iterator_tag,
            std::forward_iterator_tag)
        {
            return boost::size(rng2) >= boost::size(rng1);
        }

    } // namespace range_detail

    template<
        class SinglePassRange1,
        class SinglePassRange2,
        class Value
    >
    inline Value inner_product(
        const SinglePassRange1& rng1,
        const SinglePassRange2& rng2,
        Value                   init)
    {
        BOOST_RANGE_CONCEPT_ASSERT((
            SinglePassRangeConcept<const SinglePassRange1>));

        BOOST_RANGE_CONCEPT_ASSERT((
            SinglePassRangeConcept<const SinglePassRange2>));

        BOOST_ASSERT(
            range_detail::inner_product_precondition(
                    rng1, rng2,
                    typename range_category<const SinglePassRange1>::type(),
                    typename range_category<const SinglePassRange2>::type()));

        return std::inner_product(
            boost::begin(rng1), boost::end(rng1),
            boost::begin(rng2), init);
    }

    template<
        class SinglePassRange1,
        class SinglePassRange2,
        class Value,
        class BinaryOperation1,
        class BinaryOperation2
    >
    inline Value inner_product(
        const SinglePassRange1& rng1,
        const SinglePassRange2& rng2,
        Value                   init,
        BinaryOperation1        op1,
        BinaryOperation2        op2)
    {
        BOOST_RANGE_CONCEPT_ASSERT((
                SinglePassRangeConcept<const SinglePassRange1>));

        BOOST_RANGE_CONCEPT_ASSERT((
                SinglePassRangeConcept<const SinglePassRange2>));

        BOOST_ASSERT(
            range_detail::inner_product_precondition(
                rng1, rng2,
                typename range_category<const SinglePassRange1>::type(),
                typename range_category<const SinglePassRange2>::type()));

        return std::inner_product(
            boost::begin(rng1), boost::end(rng1),
            boost::begin(rng2), init, op1, op2);
    }

    template<class SinglePassRange, class OutputIterator>
    inline OutputIterator partial_sum(const SinglePassRange& rng,
                                      OutputIterator result)
    {
        BOOST_RANGE_CONCEPT_ASSERT((
                SinglePassRangeConcept<const SinglePassRange>));

        return std::partial_sum(boost::begin(rng), boost::end(rng), result);
    }

    template<class SinglePassRange, class OutputIterator, class BinaryOperation>
    inline OutputIterator partial_sum(
        const SinglePassRange&  rng,
        OutputIterator          result,
        BinaryOperation         op)
    {
        BOOST_RANGE_CONCEPT_ASSERT((
                SinglePassRangeConcept<const SinglePassRange>));

        return std::partial_sum(boost::begin(rng), boost::end(rng), result, op);
    }

    template<class SinglePassRange, class OutputIterator>
    inline OutputIterator adjacent_difference(
        const SinglePassRange&  rng,
        OutputIterator          result)
    {
        BOOST_RANGE_CONCEPT_ASSERT((
                SinglePassRangeConcept<const SinglePassRange>));

        return std::adjacent_difference(boost::begin(rng), boost::end(rng),
                                        result);
    }

    template<class SinglePassRange, class OutputIterator, class BinaryOperation>
    inline OutputIterator adjacent_difference(
        const SinglePassRange&  rng,
        OutputIterator          result,
        BinaryOperation         op)
    {
        BOOST_RANGE_CONCEPT_ASSERT((
                SinglePassRangeConcept<const SinglePassRange>));

        return std::adjacent_difference(boost::begin(rng), boost::end(rng),
                                        result, op);
    }

} // namespace boost

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 complex.operator toolbox - i/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.operator components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 18/02/2011
///
#include <nt2/include/constants/i.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/toolbox/constant/constant.hpp>

NT2_TEST_CASE_TPL ( i_real__2_0,  BOOST_SIMD_REAL_TYPES)
{

  typedef std::complex<T> cT;
  typedef nt2::imaginary<T> ciT;

  // specific values tests
   NT2_TEST_EQUAL(nt2::I<ciT>(), ciT(1));
   NT2_TEST_EQUAL(nt2::I<cT>(),  cT(0,1) );

} // end of test for floating_


#include <vector>
#include <string>

#include "util.hpp"

#include <rw/math/Random.hpp>
#include <rwlibs/task/GraspTask.hpp>

#include <boost/program_options/parsers.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/variables_map.hpp>
#define BOOST_FILESYSTEM_VERSION 3
#include <boost/filesystem.hpp>

using namespace std;
using namespace rw::math;
using namespace rwlibs::algorithms;
using namespace rwlibs::task;
using namespace boost::filesystem;
using namespace boost::program_options;

variables_map init(int argc, char** argv){
    options_description desc("Allowed options");
    desc.add_options()
        ("help", "This script filters a grasp list by iteratively selecting the best grasp "
                 "and removing all neighboring grasps within a 6D cube arounf the selected "
                 "grasp. The 6D cube is defined by angle and dist arguments.")
        ("output,o", value<string>()->default_value("out.xml"), "the output file.")
        ("angle", value<double>()->default_value(30.0), "Angle of cone in degree.")
        ("dist", value<double>()->default_value(0.01), "Dist of square in meters.")
        ("samples", value<int>()->default_value(500), "Number of grasps to sample.")
        ("input", value<string>(), "The input grasp task file that should be filtered.")
    ;
    positional_options_description optionDesc;
    optionDesc.add("input",-1);

    variables_map vm;
    //store(parse_command_line(argc, argv, desc), vm);
    store(command_line_parser(argc, argv).
              options(desc).positional(optionDesc).run(), vm);
    notify(vm);

    // write standard welcome, status
    if (vm.count("help")) {
        cout << "Usage:\n\n"
                  << "\t" << argv[0] <<" [options] -o<outfile> <expFile1> <expFile2> <...> <expFileN> \n"
                  << "\n";
        cout << desc << "\n";
        abort();
    }

    return vm;
}

int main_jaj(int argc, char** argv);
int main_lpe(int argc, char** argv);

int main(int argc, char** argv){
    //return main_jaj(argc,argv);
    return main_lpe(argc,argv);
}

/**
 * We need to choose a number of grasps
 */
int main_lpe(int argc, char** argv)
{
	static const unsigned int SEED = static_cast<unsigned int>(time(NULL));
	Random::seed(SEED);
	srand(SEED);
    variables_map vm = init(argc, argv);

    std::string grasptask_file_out = vm["output"].as<std::string>();
    double angleThres = vm["angle"].as<double>()*Deg2Rad;
    double distThres = vm["dist"].as<double>();
    std::string input = vm["input"].as<string>();
    std::string output = vm["output"].as<string>();
    //Unused: int count = vm["samples"].as<int>();
    GraspTask::Ptr gtask = GraspTask::load( input );

    if( boost::filesystem::exists(output) ){
        std::cout << "Output allready exists! skipping filtering." << std::endl;
        return 0;
    }

    // first we build a search tree to efficiently search for targets in 6d
    std::vector<GTaskNNSearch::Node*> simnodes;
    std::cout << "Building tree!" << std::endl;
    GTaskNNSearch *nntree = buildKDTree_pos_eaa(gtask, simnodes);
    std::cout << "sim nodes:"  << simnodes.size() << std::endl;

    Q diff(7, distThres, distThres, distThres, angleThres, angleThres, angleThres,angleThres);
    std::cout << diff << std::endl;
    typedef std::pair<GraspSubTask*, GraspTarget*> Value;
    std::vector< Value > selGrasps;
    std::list<const GTaskNNSearch::Node*> result;

    for(std::size_t i=0;i<simnodes.size(); i++){
        if(simnodes[i]->value.second == NULL)
            continue;
        // find the node with the highest quality
        double q_max = -100;
        GTaskNNSearch::Node *n_max = simnodes[0];

        // TODO: use a priority queue instead of this brute search
        int idx = 0;
        for(GTaskNNSearch::Node* nptr : simnodes) {
            GTaskNNSearch::Node& n = *nptr;
            idx++;
            GTaskNNSearch::Value &val = n.value;
            if(val.second==NULL){
                continue;
            }
            if(val.second->result==NULL)
                continue;
            Q qual = val.second->getResult()->qualityAfterLifting;
            if(qual.size()==0)
                qual = val.second->getResult()->qualityBeforeLifting;

            if(qual.size()==0)
                continue;
            if(q_max<qual[0]){
                //std::cout << "found higher:" << qual[0] << " idx: " << idx << std::endl;
                q_max = qual[0];
                n_max = &n;
            }
        }
        // just to make sure we found a node above....
        if(q_max<-90)
            break;
        // we stop the sampling at a certain threshold 30% success
        //if(q_max<0.3)
        //    break;

        // add the max node
        GTaskNNSearch::Value &v = n_max->value;
        selGrasps.push_back( v );
        v.second = NULL;

        // use the max node as a sample point and remove all neighboring nodes
        result.clear();
        nntree->nnSearchRect( n_max->key-diff, n_max->key+diff, result);
        //std::cout << "s:" << result.size() << "\n";
        for(const GTaskNNSearch::Node* res_n : result) {
            nntree->removeNode( res_n->key );
            GTaskNNSearch::Node* r_n = (GTaskNNSearch::Node*) res_n;
            r_n->value.second = NULL;
        }
    }
    std::cout << "GENRATED: "<< selGrasps.size() << " from " << simnodes.size() << std::endl;

    // convert selectGrasps to a graspTask
    GraspTask res = *gtask;
    res.getSubTasks().clear();
    for(Value& taskpair : selGrasps) {
        GraspSubTask stask = *taskpair.first;
        stask.targets.clear();
        stask.targets.push_back( *taskpair.second );
        res.addSubTask(stask);
    }

    GraspTask::saveRWTask(&res, output);
    //std::cout << "COVERAGE: " << (((double)samples_t)/(double)samples_f)*100.0 << "%" << std::endl;
    return 0;
}

/**
 * We need to choose a number of grasps
 */
int main_jaj(int argc, char** argv)
{
	static const unsigned int SEED = static_cast<unsigned int>(time(NULL));
	Random::seed(SEED);
	srand(SEED);
    variables_map vm = init(argc, argv);

	std::string grasptask_file_out = vm["output"].as<std::string>();
	double angleThres = vm["angle"].as<double>()*Deg2Rad;
    std::string input = vm["input"].as<string>();
    std::string output = vm["output"].as<string>();
    int count = vm["samples"].as<int>();
	GraspTask::Ptr gtask = GraspTask::load( input );

	// first we build a search tree to efficiently search for targets orientet in a certain direction
	GTaskNNSearch *nntree_normal = buildKDTree_eaa(gtask);

	// now we sample the normals with random orientations
	Q diff(4, angleThres, angleThres, angleThres, angleThres);
	typedef std::pair<GraspSubTask*, GraspTarget*> Value;

	std::vector< Value > selGrasps;
	std::list<const GTaskNNSearch::Node*> result;
	int samples_f = 0, samples_t = 0;
	while((int)selGrasps.size()<count){
        result.clear();
        EAA<> eaa( Math::ranRotation3D<double>() );
        //Vector3D<> k = Math::ranRotation3D<double>()*Vector3D<>::z();
        Vector3D<> k = eaa.axis();
        Q key(3, k[0], k[1], k[2], eaa.angle());
        nntree_normal->nnSearchRect(key-diff,key+diff, result);
        samples_f++;
        if(result.size()==0)
            continue;
        samples_t++;
        int idx = Random::ranI(0,boost::numeric_cast<int>(result.size()));
        if(idx == (int)result.size())
            idx--;
        std::list<const GTaskNNSearch::Node*>::iterator i = result.begin();
        //std::cout << idx << "==" <<  result.size() << std::endl;
        if(idx>0)
            std::advance(i, idx);
        const GTaskNNSearch::Node* node = *i;
        Value v = node->value;
        selGrasps.push_back( v );
        std::cout << result.size() << "    \r" << std::flush;
	}
	// print all failures

	// convert selectGrasps to a graspTask
	GraspTask res = *gtask;
	res.getSubTasks().clear();
	for(Value& taskpair : selGrasps) {
	    GraspSubTask stask = *taskpair.first;
	    stask.targets.clear();
	    stask.targets.push_back( *taskpair.second );
	    res.addSubTask(stask);
	}

	GraspTask::saveRWTask(&res, output + ".rwtask.xml");
	std::cout << "COVERAGE: " << (((double)samples_t)/(double)samples_f)*100.0 << "%" << std::endl;
	return 0;
}


/*! \file auto_1d_plot.cpp
    \brief An example to demonstrate simple 1D settings, including auto-scaling.
    \details See auto_1d_containers.cpp for an example autoscaling with multiple data-series.
    See also demo_1d_plot.cpp for a wider range of use.
*/

// Copyright Jacob Voytko 2007
// Copyright Paul A Bristow 2008, 2009, 2020

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// This file is written to be included from a Quickbook .qbk document.
// It can be compiled by the C++ compiler, and run. Any output can
// also be added here as comment or included or pasted in elsewhere.
// Caution: this file contains Quickbook markup as well as code
// and comments: don't change any of the special comment markups!

//[auto_1d_plot_1

/*`First we need a few includes to use Boost.Plot
(and some others only needed for this example).
*/

#include <boost/svg_plot/svg_1d_plot.hpp>
  using namespace boost::svg;
  using boost::svg::svg_1d_plot;

#include <boost/svg_plot/show_1d_settings.hpp>
// Only needed for showing which settings in use.
// void boost::svg::show_1d_plot_settings(svg_1d_plot&);
 // (Also provides operator<< for std::pair).

#include <boost/algorithm/minmax.hpp>
 //using boost::minmax;
#include <boost/algorithm/minmax_element.hpp>
 //using boost::minmax_element;

#include <iostream> // for debugging.
  //using std::cout;
  //using std::endl;
  //using std::boolalpha;

#include <limits>
//  using std::numeric_limits;

#include <vector>
//  using std::vector;
#include <set>
//  using std::multiset;

#include <utility>
//  using std::pair;

#include <boost/svg_plot/detail/auto_axes.hpp>
 // using boost::svg::show; // A single STL container.
 // using boost::svg::show_all; // Multiple STL containers.
 //// using boost::svg::range; // Find min and max of a STL container.
 // using boost::svg::range_all;// Find min and max of multipler STL containers.

//] [/auto_1d_plot_1]

//void scale_axis(double min_value, double max_value, // input
//               double* axis_min_value,  double* axis_max_value, double* axis_tick_increment, // updated.
//               bool origin, double tight, int min_ticks, int steps);

static const double tol100eps = 1000 * std::numeric_limits<double>::epsilon(); // suitable tight value.

int main()
{
  using boost::svg::show; // A single STL container.
  using boost::svg::show_all; // Multiple STL containers.
 // using boost::svg::range; // Find min and max of a STL container.
  using boost::svg::range_all;// Find min and max of multipler STL containers.

//[auto_1d_plot_2
  /*`This example uses containers to demonstrate autoscaling.
  Autoscaling must inspect the container in order to find axis ranges that will be suitable.
  First we create a container and fill with some fictional data.
  */
  std::vector<double> my_data;
  // Initialize my_data with some entirely fictional data.
  my_data.push_back(0.2);
  my_data.push_back(1.1); // [1]
  my_data.push_back(4.2); // [2]
  my_data.push_back(3.3); // [3]
  my_data.push_back(5.4); // [4]
  my_data.push_back(6.5); // [5]

  /*`Also included is an 'at limit' value that could confuse autoscaling.
  Obviously, we do *not* want the plot range to include infinity.*/
  my_data.push_back(std::numeric_limits<double>::infinity()); // [6]

  try
  { // Ensure error, warning and information messages are displayed by the catch block.
    double mn;
    double mx;
    int good = mnmx(my_data.begin(), my_data.end(), &mn, &mx);
    std::cout << good << " good values, " << my_data.size() - good << " limit values." << std::endl;

    using boost::svg::detail::operator<<; // For displaying std::pair.
    svg_1d_plot my_1d_plot; // Construct a plot with all the default constructor values.
    my_1d_plot.x_autoscale(my_data);  // Compute autoscale values for the plot.
    my_1d_plot.nan_limit_color(blue).nan_limit_fill_color(green); // Add NaNlimit value styling.
    //my_1d_plot.plus_inf_limit_color(blue).plus_inf_limit_fill_color(green); // Add + infinity limit value styling.
    //my_1d_plot.minus_inf_limit_color(blue).minus_inf_limit_fill_color(green); // Add - infinity limit value styling.

    std::cout << "my_1d_plot.limit_color() " << my_1d_plot.nan_limit_color() << std::endl;
    std::cout << "my_1d_plot.limit_fill_color() " << my_1d_plot.nan_limit_fill_color() << std::endl;

    my_1d_plot.plot(my_data, "Default 1D"); // Add the one data-series, and give it a title.
    my_1d_plot.write("auto_1d_plot_1.svg"); // Write the plot to file.

    /*`It may be useful to display that range chosen by autoscaling. */
    std::cout << "x_range() " << my_1d_plot.x_range() << std::endl; // x_range()
//] [/auto_1d_plot_2]
  }
  catch(const std::exception& e)
  { // Error, warning and information messages are displayed by the catch block.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }

//[auto_1d_plot_3
/*`Other STL containers can also be used, for example set, or multiset (allowing duplicates):*/
  try
  { // Ensure error, warning and information messages are displayed by the catch block.

  std::multiset<double> my_set;
  // Initialize my_set with some entirely fictional data.
  my_set.insert(1.2);
  my_set.insert(2.3);
  my_set.insert(3.4);
  my_set.insert(4.5);
  my_set.insert(5.6);
  my_set.insert(6.7);
  my_set.insert(7.8);
  my_set.insert(8.9);
  // Some examples of showing the set using various iterators:
  std::multiset<double>::const_iterator si;

  show(my_data); // Entire container contents,
  show(my_set); // for two different types of container.

  // show(&my_data[0], &my_data[my_data.size()]); // pointers - wrong! uses more all data ;-)
  show(&my_data[0], &my_data[my_data.size()-1]); // pointers, all data.
  show(&my_data[1], &my_data[5]); // pointers, part data.
  show(my_data.begin(), my_data.end()); // iterators.
  show(++(my_data.begin()), --(my_data.end())); // Just the 4 middle values.

  std::vector<double>::const_iterator idb = my_data.begin();
  std::vector<double>::const_iterator ide = my_data.end();
  show(idb, ide); // All
  ++idb; // Move to 2nd value.
  --ide; // move back from last value.
  show(idb, ide); // Just the 4 middle values.

  typedef std::vector<double>::const_iterator vector_iterator;
  std::pair<vector_iterator, vector_iterator> result = boost::minmax_element(my_data.begin(), my_data.end());
  std::cout << "The smallest element is " << *(result.first) << std::endl; // 0.2
  std::cout << "The largest element is  " << *(result.second) << std::endl; // 6.5

/*`Autoscaling can also use two `double` min and max values provided by the user program.
Using `x_autoscale(my_set)` effectively uses the first and last items in the STL container.
If the container is sorted, then these are the minimum and maximum values.*/

  double min_value = *(my_data.begin());
  double max_value = *(--my_data.end());
  std::cout << "my_set min " << min_value << ", max = " << max_value << std::endl;

/*`Function `scale_axis` is used by autoscale, but is also available for use direct by the user.
It accepts parameters controlling the scaling and updates 4 items. Its signature is:

  void scale_axis(
    double min_value, // Input range min
    double max_value, // Input range max
    double* axis_min_value,  double* axis_max_value, double* axis_tick_increment, int* auto_ticks, // All 4 updated.
    bool check_limits, // Whether to check all values for infinity, NaN etc.
    bool origin, // If true, ensures that zero is a tick value.
    double tight, // Allows user to avoid a small fraction over a tick using another tick.
    int min_ticks, // Minimum number of ticks.
*/
  double axis_min_value; // Values to be updated by autoscale.
  double axis_max_value;
  double axis_tick_increment;
  int axis_ticks;

/*`
The values of `min_value` and `max_value`
could be provided by the user program:
but usually these values are derived in some way from the user data.
Several examples follow: */

  scale_axis(1., 9., // User chosen min and max.
    &axis_min_value, &axis_max_value, &axis_tick_increment, &axis_ticks,
    false, tol100eps, 6, 0); // Display range.
  std::cout << "scaled min " << axis_min_value << ", max = " << axis_max_value
    << ", increment " << axis_tick_increment << ", axis ticks " << axis_ticks << std::endl;

  // Scaling using only the first and last values in a container,
  // assuming the data are already ordered in ascending value,
  // for example, set, map, multimap, or a sorted vector or array.
  // scale_axis(*my_data.begin(),*(--my_data.end()),

  // Scaling using two begin & end iterators into STL container,
  // scale_axis does finding min and max.
  scale_axis(my_data.begin(), my_data.end(),
    &axis_min_value, &axis_max_value, &axis_tick_increment, &axis_ticks,
    true, 3., false, tol100eps, 6); // Display range.
  std::cout << "scaled min " << axis_min_value << ", max = " << axis_max_value
    << ", increment " << axis_tick_increment << ", axis ticks " << axis_ticks << std::endl;

  // Scaling using two begin & end iterators into STL container,
  // scale_axis does finding min and max.
  scale_axis(my_data[1], my_data[4], // Only middle part of the container used, ignoring 1st and last values.
    &axis_min_value, &axis_max_value, &axis_tick_increment, &axis_ticks,
    false, tol100eps, 6); // Display range.
  std::cout << "scaled min " << axis_min_value << ", max = " << axis_max_value
    << ", increment " << axis_tick_increment << ", axis ticks " << axis_ticks << std::endl;

  // Scaling using whole  STL vector container,
  // scale_axis does finding min and max.
  scale_axis(my_data, &axis_min_value, &axis_max_value, &axis_tick_increment, &axis_ticks,
    true, 3., false, tol100eps, 6); // Display range.
  std::cout << "scaled min " << axis_min_value << ", max = " << axis_max_value
    << ", increment " << axis_tick_increment << ", axis ticks " << axis_ticks << std::endl;

  // Scaling using whole STL set container,
  // scale_axis does finding min and max.
  scale_axis(my_set, &axis_min_value, &axis_max_value, &axis_tick_increment, &axis_ticks,
    true, 3., false, tol100eps, 6); // Display range.
  std::cout << "scaled min " << axis_min_value << ", max = " << axis_max_value
    << ", increment " << axis_tick_increment << ", axis ticks " << axis_ticks << std::endl;
/*`
However autoscaling may go wrong if the data could contain values that are outside normal limits.
Infinity (+ and -), and maximum value, and NaN (Not A Number),
are separated by the plot program to allow them to be shown
but separate from 'normal' values. These values similarly can distort automatic scaling:
a single infinity would result in useless scaling!
When the plot range is set, the maximum and minimum values are checked,
and if not normal then an exception will be thrown, and no plot will be produced.
However, when autoscaling, it is more useful to ignore 'limit' values.
But this means checking all values individually.  If it known that all values are normal,
for example because they come from some measuring equipment that is known only to produce normal values,
it will be much quicker to use `std::min_max_element` which can take advantange of knowledge of the container.

The function `autoscale_check_limits(bool)` is provided to control this.
If set true, all values will be checked, and those 'at limits' will be ignored in autoscaling.
The default is true, to check all values.

If we had many very known normal values to plot and want to autoscale,
we might instead opt to ignore these checks, and write:
*/
  svg_1d_plot my_1d_plot; // Construct a plot with all the default constructor values.

  //my_1d_plot.autoscale_check_limits(false);
  // This *will throw an exception* if checks are avoided and any values are at 'limits'.

  // One could also intercept and change any values calculated by scale_axis here.
  // Set the plot to use range and interval from the scale_axis values.
  my_1d_plot.x_range(axis_min_value, axis_max_value)
    .x_major_interval(axis_tick_increment);

  my_1d_plot.x_autoscale(false);  // Ensure autoscale values are *not* recalculated for the plot.

/*`There are also some parameters which can fine-tune the autoscaling
to produce more aesthetically pleasing ranges. One can:

# Enforce the inclusion of zero on the plot.
# Specify a minimum number of major ticks.
# Specify the steps between major ticks, default 0,  or 2 for 2, 4, 6, 8, 10, 5 for 1, 5, 10, or 10 (2, 5, 10).
# Avoid values that are a tiny amount over the minimum or maximum from causing
  an apparently unnecessary tick at the minimum or maximum.
*/

  // Set some autoscaling parameters:
  my_1d_plot.x_with_zero(false);
  my_1d_plot.x_min_ticks(10);
  my_1d_plot.x_steps(0);
  my_1d_plot.x_tight(0.001);

  // Show the flags just set.
  std::cout << (my_1d_plot.x_with_zero() ? "x_with_zero, " : "not x_with_zero, ")
    << my_1d_plot.x_min_ticks() << " x_min_ticks, "
    << my_1d_plot.x_steps() << " x_steps, "
    << my_1d_plot.x_tight() << " tightness." << std::endl;

/*`Finally here are some examples of using autoscaling using all or part of containers.
*/
  my_1d_plot.x_autoscale(my_data);  // Use all my_data to autoscale.
  std::cout << "Autoscaled " // Show the results of autoscale:
    "min " << my_1d_plot.x_auto_min_value()
    << ", max "<< my_1d_plot.x_auto_max_value()
    << ", interval " << my_1d_plot.x_auto_tick_interval() << std::endl; // Autoscaled min 0, max 6.5, interval 0.5

  my_1d_plot.x_autoscale(my_data.begin(), my_data.end());  // Use all my_data to autoscale.

  std::cout << "Autoscaled " // Show the results of autoscale:
    "min " << my_1d_plot.x_auto_min_value()
    << ", max "<< my_1d_plot.x_auto_max_value()
    << ", interval " << my_1d_plot.x_auto_tick_interval() << std::endl; // Autoscaled min 0, max 6.5, interval 0.5

  my_1d_plot.x_autoscale(my_data[1], my_data[4]);  // Use only part of my_data to autoscale.

  std::cout << "Autoscaled " // Show the results of autoscale:
    "min " << my_1d_plot.x_auto_min_value()
    << ", max "<< my_1d_plot.x_auto_max_value()
    << ", interval " << my_1d_plot.x_auto_tick_interval() << std::endl; // Autoscaled min 1, max 5.5, interval 0.5

  //my_1d_plot.x_autoscale(true);  // Ensure autoscale values are used for the plot.
  // This is also automatically set true by any call of x_autoscale(some_data);

/*`The actual addition of data values to the plot is, of course, quite separate from any autoscaling.
*/
  my_1d_plot.plot(my_data, "Auto 1D"); // Add the one data-series.
  std::cout << "Autoscaled " // Show the results of autoscale:
    " min " << my_1d_plot.x_auto_min_value()
    << ", max "<< my_1d_plot.x_auto_max_value()
    << ", interval " << my_1d_plot.x_auto_tick_interval() << std::endl; // Autoscaled min 1, max 5.5, interval 0.5

  my_1d_plot.plot(my_set.begin(), my_set.end(), "Auto 1D"); // Add another data-series from my_set.
  my_1d_plot.plot(my_set, "Auto 1D"); // Add another whole data-series from my_set.
  my_1d_plot.plot(&my_data[1], &my_data[4], "Auto 1D"); // Add part (1,2 3 but *not* 4) of the one data-series.
  //my_1d_plot.plot(&my_set[1], &my_set[4], "Auto 1D"); // operator[] is not defined for set container!

  my_1d_plot.write("auto_1d_plot_2.svg"); // Write the plot to file.

  using boost::svg::detail::operator<<;
  std::cout << "x_range() " << my_1d_plot.x_range() << std::endl; // x_range() 1, 5.5
//  show_1d_plot_settings(my_1d_plot); // Full diagnostics, if required.
  }
  catch(const std::exception& e)
  { // Error, warning and information messages are displayed by the catch block.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }

/*`If necessary, one can obtain a complete listing of all the settings used.
This is often useful when the plot does not meet expectations.
*/

//] [auto_1d_plot_3]

  return 0;
} // int main()

/*
//[auto_1d_plot_output

------ Rebuild All started: Project: auto_1d_plot, Configuration: Debug Win32 ------
Deleting intermediate and output files for project 'auto_1d_plot', configuration 'Debug|Win32'
Compiling...
auto_1d_plot.cpp
Linking...
Embedding manifest...
Autorun "j:\Cpp\SVG\debug\auto_1d_plot.exe"
limit value: 1.#INF
6 good values, 1 limit values.
limit value: 1.#INF
x_range() 0, 7
7 values in container: 0.2 1.1 4.2 3.3 5.4 6.5 1.#INF
8 values in container: 1.2 2.3 3.4 4.5 5.6 6.7 7.8 8.9
0.2 1.1 4.2 3.3 5.4 6.5 : 6 values used.
1.1 4.2 3.3 5.4 : 4 values used.
0.2 1.1 4.2 3.3 5.4 6.5 1.#INF : 7 values used.
1.1 4.2 3.3 5.4 6.5 : 5 values used.
0.2 1.1 4.2 3.3 5.4 6.5 1.#INF : 7 values used.
1.1 4.2 3.3 5.4 6.5 : 5 values used.
The smallest element is 0.2
The largest element is  1.#INF
my_set min 0.2, max = 1.#INF
scaled min 1, max = 9, increment 1, axis ticks 9
limit value: 1.#INF
scaled min 0, max = 7, increment 1, axis ticks 8
scaled min 1, max = 6, increment 1, axis ticks 6
limit value: 1.#INF
scaled min 0, max = 7, increment 1, axis ticks 8
scaled min 1, max = 9, increment 1, axis ticks 9
not x_with_zero, 10 x_min_ticks, 0 x_steps, 0.001 tightness.
limit value: 1.#INF
Autoscaled min 0, max 6.5, interval 0.5
limit value: 1.#INF
Autoscaled min 0, max 6.5, interval 0.5
Autoscaled min 1, max 5.5, interval 0.5
Autoscaled  min 1, max 5.5, interval 0.5
x_range() 1, 5.5

axes_on false
background_border_width 1
background_border_color RGB(255,255,0)
background_color RGB(255,255,255)
image_border_margin() 10
image_border_width() 1
coord_precision 3
copyright_date
copyright_holder
description
document_title
image_x_size 500
image_y_size 200
legend_on false
legend_place 2
legend_top_left -1, -1, legend_bottom_right -1, -1
legend_background_color blank
legend_border_color RGB(255,255,0)
legend_color blank
legend_title
legend_title_font_size 14
legend_width 0
legend_lines true
limit points stroke color RGB(119,136,153)
limit points fill color RGB(250,235,215)
license_on false
license_reproduction permits
license_distribution permits
license_attribution requires
license_commercialuse permits
plot_background_color RGB(255,255,255)
plot_border_color RGB(255,255,0)
plot_border_width 1
plot_window_on true
plot_window_x 11, 489
plot_window_x_left 11
plot_window_x_right 489
plot_window_y 56, 139
plot_window_y_top 56
plot_window_y_bottom 139.2
title_on true
title ""
title_color blank
title_font_alignment 2
title_font_decoration
title_font_family Verdana
title_font_rotation 0
title_font_size 18
title_font_stretch
title_font_style
x_value_precision 3
x_value_ioflags 200 IOS format flags (0x200) dec.
x_max 5.5
x_min 1
x_axis_on true
x_axis_color() RGB(0,0,0)
x_axis_label_color RGB(0,0,0)
x_axis_value_color RGB(0,0,0)
x_axis_width 1
x_label_on true
x_label
x_label_color blank
x_label_font_family Verdana
x_label_font_size 14
x_label_units
x_label_units_on false
x_major_labels_side left
x_values_font_size 10
x_values_color blank
x_values_precision 3
x_values_ioflags 512
x_major_label_rotation 0
x_major_grid_color RGB(200,220,255)
x_major_grid_on false
x_major_grid_width 1
x_major_interval 0.5
x_major_tick 0.5
x_major_tick_color RGB(0,0,0)
x_major_tick_length 5
x_major_tick_width 2
x_minor_interval 0
x_minor_tick_color RGB(0,0,0)
x_minor_tick_length 2
x_minor_tick_width 1
x_minor_grid_on false
x_minor_grid_color RGB(200,220,255)
x_minor_grid_width 0.5
x_range() 1, 5.5
x_num_minor_ticks 4
x_ticks_down_on true
x_ticks_up_on false
x_ticks_on_window_or_axis bottom
x_axis_position x_axis_position intersects Y axis (Y range includes zero)
x_autoscale true
x_autoscale_check_limits true
data lines width 0
//] [auto_1d_plot_output]

*/


#ifndef KMCL_FOVISION_HPP_
#define KMCL_FOVISION_HPP_

#include <iostream>
#include <stdio.h>
#include <signal.h>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include <boost/shared_ptr.hpp>
#include <boost/function.hpp>

#include <lcm/lcm-cpp.hpp>

#include <lcmtypes/bot_core.h>
#include <fovis/fovis.hpp>

#include <lcmtypes/fovis_stats_t.h>
#include <lcmtypes/fovis_update_t.h>

class FoVision
{
public:
    FoVision(boost::shared_ptr<lcm::LCM> &lcm_,
             boost::shared_ptr<fovis::StereoCalibration> kcal);
    //FoVision(boost::shared_ptr<lcm::LCM> &lcm_);

    ~FoVision();

    void doOdometry(uint8_t *left_buf,uint8_t *right_buf, int64_t utime);
    void doOdometry(uint8_t *left_buf,float *disparity_buf, int64_t utime);

    void send_status_msg(std::string text);

    void send_delta_translation_msg(Eigen::Isometry3d motion_estimate,
      Eigen::MatrixXd motion_cov, std::string channel_name);

    void fovis_stats();

    Eigen::Isometry3d getMotionEstimate(){
      Eigen::Isometry3d motion_estimate = odom_.getMotionEstimate();

      // rotate coordinate frame so that look vector is +X, and up is +Z
      Eigen::Matrix3d M;
      M <<  0,  0, 1,
	    -1,  0, 0,
	    0, -1, 0;

      motion_estimate= M * motion_estimate;
      Eigen::Vector3d translation(motion_estimate.translation());
      Eigen::Quaterniond rotation(motion_estimate.rotation());
      rotation = rotation * M.transpose();

      Eigen::Isometry3d motion_estimate_out;
      motion_estimate_out.setIdentity();
      motion_estimate_out.translation() << translation[0],translation[1],translation[2];
      motion_estimate_out.rotate(rotation);
      return motion_estimate_out;
    }

    fovis::MotionEstimateStatusCode getEstimateStatus(){
      fovis::MotionEstimateStatusCode estim_status = odom_.getMotionEstimateStatus();
      /*std::cout << estim_status << "is status\n";
      if (estim_status == fovis::SUCCESS){
        std::cout << estim_status << "is valid status\n";
      }else if (estim_status = fovis::INSUFFICIENT_INLIERS){
        std::cout << estim_status << "is FOVIS_UPDATE_T_ESTIMATE_INSUFFICIENT_FEATURES\n";
      }*/

      return estim_status;
    }

    const fovis::FeatureMatch* getMatches(){ return odom_.getMotionEstimator()->getMatches(); }
    int getNumMatches(){ return odom_.getMotionEstimator()->getNumMatches(); }
    int getNumInliers(){ return odom_.getMotionEstimator()->getNumInliers(); }

    bool getChangeReferenceFrames(){ return odom_.getChangeReferenceFrames(); }

    void getMotion(Eigen::Isometry3d &delta, Eigen::MatrixXd &delta_cov, fovis::MotionEstimateStatusCode& delta_status ){
      delta=       odom_.getMotionEstimate();
      delta_cov =  odom_.getMotionEstimateCov();
      delta_status = odom_.getMotionEstimateStatus();
    }

    Eigen::Isometry3d getPose(){
      return odom_.getPose();
    }

private:
    boost::shared_ptr<lcm::LCM> lcm_;
    boost::shared_ptr<fovis::StereoCalibration> kcal_;
    fovis::VisualOdometry odom_;

    // Depth Sources:
    fovis::StereoDepth* stereo_depth_; // typical left/right stereo
    fovis::StereoDisparity* stereo_disparity_; // left/disparity from multisense

    //fovis::PrimeSenseDepth depth_producer_; // disparity from Freenect
    //fovis::DepthImage* depth_image_; // depth from OpenNI

    float* depth_data_;
    Eigen::Isometry3d pose_;
    fovis::StereoCalibration* default_config();
    static fovis::VisualOdometryOptions getDefaultOptions();
    int64_t current_timestamp_,prev_timestamp_;
};

#endif


#include "carla/occupancy/OccupancyMap.h"
#include <vector>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <algorithm>
#include <sstream>
#include <fstream>

namespace carla {
namespace occupancy {

OccupancyMap::OccupancyMap() {

}

OccupancyMap::OccupancyMap(const std::vector<geom::Vector2D>& line, float width) {
  // Convert into b_linestring_t.
  b_linestring_t linestring;
  for (const geom::Vector2D& vertex : line) {
    boost::geometry::append(linestring, b_point_t(vertex.x, vertex.y));
  }

  // Correct geometry.
  boost::geometry::correct(linestring);

  // Calculate buffer.
  boost::geometry::strategy::buffer::distance_symmetric<float> distance_strategy(width / 2);
  boost::geometry::strategy::buffer::side_straight side_strategy;
  boost::geometry::strategy::buffer::join_round join_strategy(18);
  boost::geometry::strategy::buffer::end_round end_strategy(18);
  boost::geometry::strategy::buffer::point_circle circle_strategy(18);
  boost::geometry::buffer(linestring, _multi_polygon,
      distance_strategy, side_strategy, join_strategy, end_strategy, circle_strategy);
}

OccupancyMap::OccupancyMap(const std::vector<geom::Vector2D>& line, float width, float thickness) {
  // Convert into b_linestring_t.
  b_linestring_t linestring;
  for (const geom::Vector2D& vertex : line) {
    boost::geometry::append(linestring, b_point_t(vertex.x, vertex.y));
  }

  // Correct geometry.
  boost::geometry::correct(linestring);

  // Calculate outline.
  b_multi_polygon_t outline;
  boost::geometry::buffer(linestring, outline,
      boost::geometry::strategy::buffer::distance_symmetric<float>(width / 2),
      boost::geometry::strategy::buffer::side_straight(),
      boost::geometry::strategy::buffer::join_round(18),
      boost::geometry::strategy::buffer::end_flat(),
      boost::geometry::strategy::buffer::point_circle(18));

  b_multi_polygon_t outline_outer;
  boost::geometry::buffer(outline[0].outer(), outline_outer,
      boost::geometry::strategy::buffer::distance_symmetric<float>(thickness / 2),
      boost::geometry::strategy::buffer::side_straight(),
      boost::geometry::strategy::buffer::join_round(18),
      boost::geometry::strategy::buffer::end_flat(),
      boost::geometry::strategy::buffer::point_circle(18));

  b_multi_polygon_t outline_inner;
  boost::geometry::buffer(outline[0].outer(), outline_inner,
      boost::geometry::strategy::buffer::distance_symmetric<float>(-thickness / 2),
      boost::geometry::strategy::buffer::side_straight(),
      boost::geometry::strategy::buffer::join_round(18),
      boost::geometry::strategy::buffer::end_flat(),
      boost::geometry::strategy::buffer::point_circle(18));

  boost::geometry::difference(outline_outer, outline_inner, _multi_polygon);
}

OccupancyMap::OccupancyMap(const std::vector<geom::Vector2D>& polygon) {
  // Convert into b_multi_polygon_t.
  _multi_polygon.emplace_back();
  for (const geom::Vector2D& vertex : polygon) {
    boost::geometry::append(_multi_polygon[0].outer(), b_point_t(vertex.x, vertex.y));
  }

  // Correct geometry.
  boost::geometry::correct(_multi_polygon);
}

OccupancyMap::OccupancyMap(const geom::Vector2D& bounds_min, const geom::Vector2D& bounds_max)
  : OccupancyMap({
      bounds_min, geom::Vector2D(bounds_min.x, bounds_max.y),
      bounds_max, geom::Vector2D(bounds_max.x, bounds_min.y)}) {

}

OccupancyMap OccupancyMap::Load(const std::string& file) {
  std::ifstream ifs;
  ifs.open(file, std::ios::in);
  std::ostringstream ss_data;
  ss_data << ifs.rdbuf();
  ifs.close();
  return LoadFromWkt(ss_data.str());
}

OccupancyMap OccupancyMap::LoadFromWkt(const std::string& wkt) {
  std::stringstream ss(wkt);

  auto read_point = [&ss] (b_point_t& point) {
    std::string text;
    while (true) {
      char c; ss.get(c);
      if (c == ')') {
        break;
      } else {
        text += c;
      }
    }

    std::vector<std::string> text_split;
    boost::split(text_split, text, boost::is_any_of(" "));
    point.x(std::stof(text_split[0]));
    point.y(std::stof(text_split[1]));
  };

  auto read_ring = [&ss, &read_point] (b_ring_t& ring) {
    while (true) {
      char c; ss.get(c);
      if (c == '(') {
        b_point_t point; read_point(point);
        boost::geometry::append(ring, point);
      } else if (c == ')') {
        break;
      } else {
        // Error
      }
    }
  };

  auto read_polygon = [&ss, &read_ring] (b_polygon_t& polygon) {
    char c; ss.get(c);
    read_ring(polygon.outer());

    while (true) {
      ss.get(c);
      if (c == '(') {
        polygon.inners().resize(polygon.inners().size() + 1);
        read_ring(polygon.inners()[polygon.inners().size() - 1]);
      } else if (c == ')') {
        break;
      } else {
        // Error
      }
    }
  };

  auto read_multi_polygon = [&ss, &read_polygon] (b_multi_polygon_t& multi_polygon) {
    char c; ss.get(c); // ( of multi_polygon.

    while (true) {
      ss.get(c); // ( of polygon, or ).
      if (c == '(') {
        multi_polygon.resize(multi_polygon.size() + 1);
        read_polygon(multi_polygon[multi_polygon.size() - 1]);
      } else if (c == ')') {
        break;
      } else {
        // Error
      }
    }
  };

  OccupancyMap occupancy_map;
  read_multi_polygon(occupancy_map._multi_polygon);
  boost::geometry::correct(occupancy_map._multi_polygon);

  return occupancy_map;
}

void OccupancyMap::Save(const std::string& file) const {
  std::ofstream ofs;
  ofs.open(file, std::ios::out | std::ios::trunc);

  ofs << '(';
  for (const b_polygon_t& polygon : _multi_polygon) {
    ofs << '(';

    ofs << '(';
    for (const b_point_t& point : polygon.outer()) {
      ofs << '(';
      ofs << std::to_string(point.x()) << ' ' << std::to_string(point.y());
      ofs << ')';
    }
    ofs << ')';

    for (const b_ring_t& inner : polygon.inners()) {
      ofs << '(';
      for (const b_point_t& point : inner) {
        ofs << '(';
        ofs << std::to_string(point.x()) << ' ' << std::to_string(point.y());
        ofs << ')';
      }
      ofs << ')';
    }

    ofs << ')';
  }
  ofs << ')';

  ofs.close();
}

bool OccupancyMap::IsEmpty() const {
  return boost::geometry::is_empty(_multi_polygon);
}

bool OccupancyMap::operator==(const OccupancyMap& occupancy_map) const {
  return boost::geometry::equals(_multi_polygon, occupancy_map._multi_polygon);
}

bool OccupancyMap::operator!=(const OccupancyMap& occupancy_map) const {
  return !(*this == occupancy_map);
}

OccupancyMap OccupancyMap::Union(const OccupancyMap& occupancy_map) const {
  OccupancyMap result;
  boost::geometry::union_(_multi_polygon, occupancy_map._multi_polygon, result._multi_polygon);
  return result;
}

OccupancyMap OccupancyMap::Difference(const OccupancyMap& occupancy_map) const {
  OccupancyMap result;
  boost::geometry::difference(_multi_polygon, occupancy_map._multi_polygon, result._multi_polygon);
  return result;
}

OccupancyMap OccupancyMap::Intersection(const OccupancyMap& occupancy_map) const {
  OccupancyMap result;
  boost::geometry::intersection(_multi_polygon, occupancy_map._multi_polygon, result._multi_polygon);
  return result;
}

OccupancyMap OccupancyMap::Buffer(float width) const {
  OccupancyMap result;

  boost::geometry::strategy::buffer::distance_symmetric<float> distance_strategy(width / 2);
  boost::geometry::strategy::buffer::join_round join_strategy(18);
  boost::geometry::strategy::buffer::end_round end_strategy(18);
  boost::geometry::strategy::buffer::point_circle circle_strategy(18);
  boost::geometry::strategy::buffer::side_straight side_strategy;
  boost::geometry::buffer(_multi_polygon, result._multi_polygon,
      distance_strategy, side_strategy, join_strategy, end_strategy, circle_strategy);

  return result;
}

bool OccupancyMap::Contains(const geom::Vector2D& point) const {
  return boost::geometry::covered_by(b_point_t(point.x, point.y), _multi_polygon);
}

bool OccupancyMap::Intersects(const OccupancyMap& occupancy_map) const {
  return boost::geometry::intersects(_multi_polygon, occupancy_map._multi_polygon);
}

sidewalk::Sidewalk OccupancyMap::CreateSidewalk(float distance) const {
  std::vector<std::vector<geom::Vector2D>> polygons;

  for (const b_polygon_t& polygon : _multi_polygon) {

    // Calculate outer buffer.
    OccupancyMap outer_map;
    outer_map._multi_polygon.emplace_back(b_polygon_t({polygon.outer()}));
    boost::geometry::correct(outer_map._multi_polygon);
    outer_map = outer_map.Buffer(distance);

    for (const b_polygon_t& buffer_polygon : outer_map._multi_polygon) {

      // Add outer of outer buffer in clockwise order.
      polygons.emplace_back();
      for (const b_point_t& vertex : buffer_polygon.outer()) {
        polygons.back().emplace_back(vertex.x(), vertex.y());
      }
      std::reverse(polygons.back().begin(), polygons.back().end());

      // Add inners of outer buffer in anticlockwise order.
      // These possibly form positive buffering a convex polygon, leading to overlaps.
      for (const b_ring_t& buffer_polygon_inner : buffer_polygon.inners()) {
        polygons.emplace_back();
        for (const b_point_t& vertex : buffer_polygon_inner) {
          polygons.back().emplace_back(vertex.x(), vertex.y());
        }
        std::reverse(polygons.back().begin(), polygons.back().end());
      }
    }

    for (const b_ring_t& inner : polygon.inners()) {

      // Calculate inner buffer.
      OccupancyMap inner_map;
      inner_map._multi_polygon.emplace_back(b_polygon_t({inner}));
      boost::geometry::correct(inner_map._multi_polygon);
      inner_map = inner_map.Buffer(-distance);

      // Add outer of inner buffer in anticlockwise order.
      // If polygon is too small for negative buffering, then this will be empty.
      for (const b_polygon_t& buffer_polygon : inner_map._multi_polygon) {
        polygons.emplace_back();
        for (const b_point_t& vertex : buffer_polygon.outer()) {
          polygons.back().emplace_back(vertex.x(), vertex.y());
        }
      }
    }

  }

  return sidewalk::Sidewalk(std::move(polygons));
}

std::vector<std::vector<std::vector<geom::Vector2D>>> OccupancyMap::GetPolygons() const {
  std::vector<std::vector<std::vector<geom::Vector2D>>> vertices;

  for (const b_polygon_t polygon : _multi_polygon) {
    vertices.emplace_back(); // New polygon.

    vertices.back().emplace_back(); // New ring.
    for (const b_point_t vertex : polygon.outer()) {
      vertices.back().back().emplace_back(vertex.x(), vertex.y()); // New point.
    }

    for (const b_ring_t ring : polygon.inners()) {
      vertices.back().emplace_back(); // New ring.
      for (const b_point_t vertex : ring) {
        vertices.back().back().emplace_back(vertex.x(), vertex.y()); // New point.
      }
    }
  }

  return vertices;
}

}
}


#include "denserasternonsimd.h"
#include "gdx/algo/sum.h"
#include "gdx/denseraster.h"
#include "gdx/maskedraster.h"
#include "gdx/rasteriterator.h"

#include <Eigen/Core>

#include <benchmark/benchmark.h>
#include <iostream>
#include <numeric>
#include <stdexcept>

using namespace gdx;

template <typename T>
using EigenArray = Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

template <typename T>
EigenArray<T> make_eigen_array(int32_t rows, int32_t cols, T fillValue)
{
    EigenArray<T> arr(rows, cols);
    arr.fill(fillValue);
    return arr;
}

template <typename Raster>
static void add_2_rasters(benchmark::State& state)
{
    using T  = typename Raster::value_type;
    auto dim = inf::truncate<int32_t>(state.range(0));
    Raster ras1(RasterMetadata(dim, dim, -1.0), T(1));
    Raster ras2(RasterMetadata(dim, dim, -1.0), T(2));
    for (auto _ : state) {
        benchmark::DoNotOptimize(ras1 + ras2);
    }
}

static void add_2_rasters_eigen(benchmark::State& state)
{
    auto dim  = inf::truncate<int32_t>(state.range(0));
    auto ras1 = make_eigen_array<int32_t>(dim, dim, 1);
    auto ras2 = make_eigen_array<int32_t>(dim, dim, 2);
    for (auto _ : state) {
        // Assign to an eigen array to evaluate the expression template
        EigenArray<int> res = ras1 + ras2;
        (void)res;
    }
}

template <typename Raster>
static void add_2_rasters_inplace(benchmark::State& state)
{
    using T  = typename Raster::value_type;
    auto dim = inf::truncate<int32_t>(state.range(0));
    Raster ras1(RasterMetadata(dim, dim, -1.0), T(1));
    Raster ras2(RasterMetadata(dim, dim, -1.0), T(2));
    for (auto _ : state) {
        benchmark::DoNotOptimize(ras1 += ras2);
    }
}

template <typename T>
static void add_2_rasters_inplace_eigen(benchmark::State& state)
{
    auto dim  = inf::truncate<int32_t>(state.range(0));
    auto ras1 = make_eigen_array<T>(dim, dim, T(1));
    auto ras2 = make_eigen_array<T>(dim, dim, T(2));
    for (auto _ : state) {
        ras1 += ras2;
    }
}

static void rasterAddScalarInt(benchmark::State& state)
{
    auto dim = inf::truncate<int32_t>(state.range(0));
    DenseRaster<int> ras(RasterMetadata(dim, dim, -1.0), 1);
    for (auto _ : state) {
        auto res = ras + 5;
        if (res[0] != 6) {
            throw std::runtime_error("");
        }
    }
}

static void rasterAddScalarFloat(benchmark::State& state)
{
    auto dim = inf::truncate<int32_t>(state.range(0));
    DenseRaster<float> ras(RasterMetadata(dim, dim, -1.0), 1.f);
    for (auto _ : state) {
        auto res = ras + 5.f;
        if (res[0] != 6.f) {
            throw std::runtime_error("");
        }
    }
}

static void rasterAddScalarFloatEigen(benchmark::State& state)
{
    auto dim = inf::truncate<int32_t>(state.range(0));
    auto ras = make_eigen_array<float>(dim, dim, 1.f);
    for (auto _ : state) {
        EigenArray<float> res = ras + 5.f;
        (void)res;
    }
}

static void rasterMultiplyInPlaceInt(benchmark::State& state)
{
    auto dim = inf::truncate<int32_t>(state.range(0));
    DenseRaster<int> ras(RasterMetadata(dim, dim, -1.0), 1);
    for (auto _ : state) {
        ras *= 5;
    }
}

static void rasterMultiplyInPlaceFloat(benchmark::State& state)
{
    auto dim = inf::truncate<int32_t>(state.range(0));
    DenseRaster<float> ras(RasterMetadata(dim, dim, -1.0), 1);
    for (auto _ : state) {
        ras *= 5.f;
    }
}

static void rasterAddInPlaceInt(benchmark::State& state)
{
    auto dim = inf::truncate<int32_t>(state.range(0));
    DenseRaster<int> ras(RasterMetadata(dim, dim, -1.0), 1);
    for (auto _ : state) {
        ras += 5;
    }
}

static void rasterAddInPlaceFloat(benchmark::State& state)
{
    auto dim = inf::truncate<int32_t>(state.range(0));
    DenseRaster<float> ras(RasterMetadata(dim, dim, -1.0), 1);
    for (auto _ : state) {
        ras += 5.f;
    }
}

static void rasterAddInPlaceFloatEigen(benchmark::State& state)
{
    auto dim = inf::truncate<int32_t>(state.range(0));
    auto ras = make_eigen_array<float>(dim, dim, 1.f);
    for (auto _ : state) {
        ras += 5.f;
    }
}

static void rasterSubtractInPlaceInt(benchmark::State& state)
{
    auto dim = inf::truncate<int32_t>(state.range(0));
    DenseRaster<int> ras(RasterMetadata(dim, dim, -1.0), 1);
    for (auto _ : state) {
        ras -= 5;
    }
}

static void rasterSubtractInPlaceFloat(benchmark::State& state)
{
    auto dim = inf::truncate<int32_t>(state.range(0));
    DenseRaster<float> ras(RasterMetadata(dim, dim, -1.0), 1);
    for (auto _ : state) {
        ras -= 5.f;
    }
}

static void rasterDivideInPlaceInt(benchmark::State& state)
{
    auto dim = inf::truncate<int32_t>(state.range(0));
    DenseRaster<int> ras(RasterMetadata(dim, dim, -1.0), 1);
    for (auto _ : state) {
        ras /= 5;
    }
}

static void rasterDivideInPlaceFloat(benchmark::State& state)
{
    auto dim = inf::truncate<int32_t>(state.range(0));
    DenseRaster<float> ras(RasterMetadata(dim, dim, -1.0), 1);
    for (auto _ : state) {
        ras /= 5.f;
    }
}

template <typename Raster>
static void fill_raster(benchmark::State& state)
{
    using T = typename Raster::value_type;

    auto dim = inf::truncate<int32_t>(state.range(0));
    Raster ras(RasterMetadata(dim, dim, -1.0), T(1));
    for (auto _ : state) {
        ras.fill(T(5));
    }
}

template <typename Raster>
static void fill_raster_values(benchmark::State& state)
{
    using T = typename Raster::value_type;

    auto dim = inf::truncate<int32_t>(state.range(0));
    Raster ras(RasterMetadata(dim, dim, -1.0), T(1));
    for (auto _ : state) {
        ras.fill_values(T(5));
    }
}

static void initFloatRasterWithData(benchmark::State& state)
{
    auto dim = inf::truncate<int32_t>(state.range(0));
    std::vector<float> v(size_t(dim * dim));
    std::iota(v.begin(), v.end(), -10.f);

    for (auto _ : state) {
        DenseRaster<float> ras(RasterMetadata(dim, dim, -1.0), v);
    }
}

#ifndef _MSC_VER
BENCHMARK_TEMPLATE(add_2_rasters, DenseRaster<uint8_t>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(add_2_rasters, nosimd::DenseRaster<int32_t>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(add_2_rasters, DenseRaster<int32_t>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(add_2_rasters, nosimd::DenseRaster<int32_t>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(add_2_rasters, DenseRaster<int32_t>)->Arg(10)->Arg(100);
#endif
BENCHMARK(add_2_rasters_eigen)->Arg(10)->Arg(100);

BENCHMARK_TEMPLATE(add_2_rasters_inplace, nosimd::DenseRaster<int32_t>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(add_2_rasters_inplace, DenseRaster<int32_t>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(add_2_rasters_inplace, nosimd::DenseRaster<float>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(add_2_rasters_inplace, DenseRaster<float>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(add_2_rasters_inplace_eigen, int32_t)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(add_2_rasters_inplace_eigen, float)->Arg(10)->Arg(100);

BENCHMARK(rasterAddScalarInt)->Arg(10)->Arg(100);
BENCHMARK(rasterAddScalarFloatEigen)->Arg(10)->Arg(100);
BENCHMARK(rasterAddScalarFloat)->Arg(10)->Arg(100);
BENCHMARK(rasterMultiplyInPlaceInt)->Arg(10)->Arg(100);
BENCHMARK(rasterMultiplyInPlaceFloat)->Arg(10)->Arg(100);
BENCHMARK(rasterAddInPlaceInt)->Arg(10)->Arg(100);
BENCHMARK(rasterAddInPlaceFloat)->Arg(10)->Arg(100);
BENCHMARK(rasterAddInPlaceFloatEigen)->Arg(10)->Arg(100);
BENCHMARK(rasterSubtractInPlaceInt)->Arg(10)->Arg(100);
BENCHMARK(rasterSubtractInPlaceFloat)->Arg(10)->Arg(100);
BENCHMARK(rasterDivideInPlaceInt)->Arg(10)->Arg(100);
BENCHMARK(rasterDivideInPlaceFloat)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(fill_raster, DenseRaster<int32_t>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(fill_raster, nosimd::DenseRaster<int32_t>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(fill_raster, DenseRaster<float>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(fill_raster, nosimd::DenseRaster<float>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(fill_raster_values, DenseRaster<int32_t>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(fill_raster_values, nosimd::DenseRaster<int32_t>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(fill_raster_values, DenseRaster<float>)->Arg(10)->Arg(100);
BENCHMARK_TEMPLATE(fill_raster_values, nosimd::DenseRaster<float>)->Arg(10)->Arg(100);
BENCHMARK(initFloatRasterWithData)->Arg(10)->Arg(100);

BENCHMARK_MAIN();


// AttBuilder.cpp
// created by Kuangdai on 8-Jun-2016
// compute attenuation parameters

#include "AttSimplex.h"
#include "AttAxiSEM.h"
#include "Parameters.h"
#include "XMPI.h"
#include <boost/algorithm/string.hpp>
#include "Attenuation1D_Full.h"
#include "Attenuation1D_CG4.h"
#include "Attenuation3D_Full.h"
#include "Attenuation3D_CG4.h"
#include "Quad.h"
#include "XMath.h"

Attenuation1D *AttBuilder::createAttenuation1D(const RDMatXN &QKp, const RDMatXN &QMu,
    RDMatXN &kp, RDMatXN &mu, const Quad *quad) const {
    // factors
    RDColX alpha, beta, gamma;
    RDMatXN dKpFact, kpFactAtt, kpFactNoAtt;
    RDMatXN dMuFact, muFactAtt, muFactNoAtt;
    computeAttFactors(QKp, QMu,
        alpha, beta, gamma,
        dKpFact, kpFactAtt, kpFactNoAtt,
        dMuFact, muFactAtt, muFactNoAtt);

    // flatten to structured
    RDMatPP kpStr, muStr;
    RDMatPP dKpFactStr, kpFactAttStr, kpFactNoAttStr;
    RDMatPP dMuFactStr, muFactAttStr, muFactNoAttStr;
    XMath::structuredUseFirstRow(kp, kpStr);
    XMath::structuredUseFirstRow(mu, muStr);
    XMath::structuredUseFirstRow(dKpFact, dKpFactStr);
    XMath::structuredUseFirstRow(dMuFact, dMuFactStr);
    XMath::structuredUseFirstRow(kpFactAtt, kpFactAttStr);
    XMath::structuredUseFirstRow(muFactAtt, muFactAttStr);
    XMath::structuredUseFirstRow(kpFactNoAtt, kpFactNoAttStr);
    XMath::structuredUseFirstRow(muFactNoAtt, muFactNoAttStr);

    // create attenuation model and change kp and mu in-place
    Attenuation1D *attPtr = 0;
    if (mUseCG4) {
        const RDRow4 &weights_cg4 = quad->computeWeightsCG4();
        RDRow4 dkp, dmu;
        std::vector<int> ipol = {1, 1, 3, 3};
        std::vector<int> jpol = {1, 3, 1, 3};
        for (int i = 0; i < 4; i++) {
            int ip = ipol[i];
            int jp = jpol[i];
            dkp(i) = weights_cg4(i) * dKpFactStr(ip, jp) * kpStr(ip, jp);
            dmu(i) = weights_cg4(i) * dMuFactStr(ip, jp) * muStr(ip, jp);
        }
        kpStr.array() *= kpFactNoAttStr.array();
        muStr.array() *= muFactNoAttStr.array();
        for (int i = 0; i < 4; i++) {
            int ip = ipol[i];
            int jp = jpol[i];
            kpStr(ip, jp) *= 1. + weights_cg4(i) * (kpFactAttStr(ip, jp) / kpFactNoAttStr(ip, jp) - 1.);
            muStr(ip, jp) *= 1. + weights_cg4(i) * (muFactAttStr(ip, jp) / muFactNoAttStr(ip, jp) - 1.);
        }
        attPtr = new Attenuation1D_CG4(mNSLS,
            alpha.cast<Real>(), beta.cast<Real>(), gamma.cast<Real>(),
            quad->getNu(), dkp.cast<Real>(), dmu.cast<Real>(), doKappa());
    } else {
        RDMatPP dkpStr = dKpFactStr.schur(kpStr);
        RDMatPP dmuStr = dMuFactStr.schur(muStr);
        kpStr.array() *= kpFactAttStr.array();
        muStr.array() *= muFactAttStr.array();
        attPtr = new Attenuation1D_Full(mNSLS,
            alpha.cast<Real>(), beta.cast<Real>(), gamma.cast<Real>(),
            quad->getNu(), dkpStr.cast<Real>(), dmuStr.cast<Real>(), doKappa());
    }

    // structured to flatten
    XMath::flattenFillWithFirstRow(kpStr, kp);
    XMath::flattenFillWithFirstRow(muStr, mu);

    return attPtr;
}

Attenuation3D *AttBuilder::createAttenuation3D(const RDMatXN &QKp, const RDMatXN &QMu,
    RDMatXN &kp, RDMatXN &mu, const Quad *quad) const {
    // factors
    RDColX alpha, beta, gamma;
    RDMatXN dKpFact, kpFactAtt, kpFactNoAtt;
    RDMatXN dMuFact, muFactAtt, muFactNoAtt;
    computeAttFactors(QKp, QMu,
        alpha, beta, gamma,
        dKpFact, kpFactAtt, kpFactNoAtt,
        dMuFact, muFactAtt, muFactNoAtt);

    // create attenuation model and change kp and mu in-place
    if (mUseCG4) {
        const RDRow4 &weights_cg4 = quad->computeWeightsCG4();
        int nr = quad->getNr();
        RDMatX4 dkp(nr, 4), dmu(nr, 4);
        std::vector<int> ipol = {1, 1, 3, 3};
        std::vector<int> jpol = {1, 3, 1, 3};
        for (int i = 0; i < 4; i++) {
            int ip = ipol[i];
            int jp = jpol[i];
            dkp.col(i) = weights_cg4(i) * dKpFact.col(nPntEdge * ip + jp).schur(kp.col(nPntEdge * ip + jp));
            dmu.col(i) = weights_cg4(i) * dMuFact.col(nPntEdge * ip + jp).schur(mu.col(nPntEdge * ip + jp));
        }
        kp.array() *= kpFactNoAtt.array();
        mu.array() *= muFactNoAtt.array();
        RDColX ones = RDColX::Ones(nr);
        for (int i = 0; i < 4; i++) {
            int ip = ipol[i];
            int jp = jpol[i];
            kp.col(nPntEdge * ip + jp).array() *= ones.array() + weights_cg4(i) * (kpFactAtt.col(nPntEdge * ip + jp).array()
                / kpFactNoAtt.col(nPntEdge * ip + jp).array() - ones.array());
            mu.col(nPntEdge * ip + jp).array() *= ones.array() + weights_cg4(i) * (muFactAtt.col(nPntEdge * ip + jp).array()
                / muFactNoAtt.col(nPntEdge * ip + jp).array() - ones.array());
        }
        return new Attenuation3D_CG4(mNSLS,
            alpha.cast<Real>(), beta.cast<Real>(), gamma.cast<Real>(),
            dkp.cast<Real>(), dmu.cast<Real>(), doKappa());
    } else {
        RDMatXN dkp = dKpFact.schur(kp);
        RDMatXN dmu = dMuFact.schur(mu);
        kp.array() *= kpFactAtt.array();
        mu.array() *= muFactAtt.array();
        return new Attenuation3D_Full(mNSLS,
            alpha.cast<Real>(), beta.cast<Real>(), gamma.cast<Real>(),
            dkp.cast<Real>(), dmu.cast<Real>(), doKappa());
    }
}

std::string AttBuilder::verbose() const {
    std::stringstream ss;
    ss << "\n=================== Attenuation Builder ====================" << std::endl;
    ss << "  Coarse Grained    =   " << (mUseCG4 ? "YES" : "NO") << std::endl;
    ss << "  Number of SLS     =   " << mNSLS << std::endl;
    ss << "  Freq. Band (Hz)   =   [" << mFmin << ", " << mFmax << "]" << std::endl;
    ss << "  Ref. Freq. (Hz)   =   " << mFref << std::endl;
    ss << "  Time Step         =   " << mDeltaT << std::endl;
    ss << "  Include QKappa    =   " << (doKappa() ? "YES" : "NO") << std::endl;
    if (legacy()) ss << "  Using SPECFEM Legacy model." << std::endl;
    ss << "=================== Attenuation Builder ====================\n" << std::endl;
    return ss.str();
}

void AttBuilder::buildInparam(AttBuilder *&attBuild, const Parameters &par,
    const AttParameters *attPar, double dt, int verbose) {
    if (attBuild) {
        delete attBuild;
    }

    // pure elastic
    if (!par.getValue<bool>("ATTENUATION") || attPar == 0) {
        attBuild = 0;
        if (verbose) {
            XMPI::cout << "\n=================== Attenuation Builder ====================" << XMPI::endl;
            XMPI::cout << "  Attenuation is turned off. "  << XMPI::endl;
            XMPI::cout << "=================== Attenuation Builder ====================\n" << XMPI::endl;
        }
        return;
    }

    // parameters
    bool useLegacy = par.getValue<bool>("ATTENUATION_SPECFEM_LEGACY");
    bool cg4 = par.getValue<bool>("ATTENUATION_CG4");
    bool dokappa = par.getValue<bool>("ATTENUATION_QKAPPA");

    if (cg4 && nPol != 4) {
        throw std::runtime_error("AttBuilder::buildInparam || "
            "ATTENUATION_CG4 is turned on but nPol is not 4.");
    }

    // create model
    if (useLegacy) {
        if (dokappa) {
            throw std::runtime_error("AttBuilder::buildInparam || "
                "Cannot include Q_Kappa when ATTENUATION_SPECFEM_LEGACY = true.");
        }
        if (attPar->mNSLS != 3) {
            throw std::runtime_error("AttBuilder::buildInparam || "
                "Number of SLSs can only be 3 when ATTENUATION_SPECFEM_LEGACY = true.");
        }
        attBuild = new AttSimplex(cg4, attPar->mNSLS, attPar->mFmin, attPar->mFmax, attPar->mFref, dt);
    } else {
        attBuild = new AttAxiSEM(cg4, attPar->mNSLS, attPar->mFmin, attPar->mFmax, attPar->mFref,
            attPar->mW, attPar->mY, dt, dokappa);
    }

    // verbose
    if (verbose) {
        XMPI::cout << attBuild->verbose();
    }
}


#include <iostream>
#include <vector>

#include <dlib/dnn.h>
#include <sampml/data.hpp>
#include <sampml/random_forest.hpp>
#include <sampml/svm_classifier.hpp>

#include "common.hpp"
#include "dnn.hpp"

int main () {
    std::cout << "TESTING:\n";
    sampml::data::reader<sample_type> data_positive_test(positive_test_data);
    sampml::data::reader<sample_type> data_negative_test(negative_test_data);

    std::vector<sample_type> testing_data;
    std::vector<unsigned long> testing_labels;
    for(auto& v : data_positive_test) {
        testing_data.push_back(v);
        testing_labels.push_back(1);
    }

    for(auto& v : data_negative_test) {
        testing_data.push_back(v);
        testing_labels.push_back(0);
    }

    sampml::trainer::svm_classifier<sample_type> svm;
    svm.deserialize(std::string(svm_model.begin(), svm_model.end()));

    sampml::trainer::random_forest<sample_type> rf;
    rf.deserialize(std::string(rf_model.begin(), rf_model.end()));

    test_network_type dnn;
    dlib::deserialize(std::string(dnn_model.begin(), dnn_model.end())) >> dnn.subnet();

    int true_positives = 0,
        false_positives = 0,
        true_negatives = 0,
        false_negatives = 0;
    dlib::running_stats<double> positive_values, negative_values;
    for (std::size_t i = 0; i < testing_data.size(); ++i)
    {
        double svm_prob = svm.test(testing_data[i]),
               rf_prob = std::clamp(rf.test(testing_data[i]), 0.0, 1.0),
               dnn_prob = dlib::mat(dnn(testing_data[i]))(1);
        double prob = (svm_prob + rf_prob + dnn_prob)/3;

        const float cutoff = 0.5;
        if(testing_labels[i] == 1) {
            if(prob > cutoff) {
                true_positives++;
            } else {
                false_negatives++;
                std::cout << "false negative: (SVM: " << svm_prob << ", RF: " << rf_prob << ", DNN: " << dnn_prob << ")\n";
            }
            positive_values.add(prob);
        } else {
            if(prob < cutoff) {
                true_negatives++;
            } else {
                false_positives++;
                std::cout << "false positive: (SVM: " << svm_prob << ", RF: " << rf_prob << ", DNN: " << dnn_prob << ")\n";
            }
            negative_values.add(prob);
        }
    }

    std::cout << "positive test set statistics: " << '\n';
    std::cout << "average: " << positive_values.mean() << ", stddev: " << positive_values.stddev() << '\n'
              << "min: " << positive_values.min() << ", max: " << positive_values.max()  << '\n'
              << "skewness: " << positive_values.skewness() << ", excess kurtosis: " << positive_values.ex_kurtosis() << '\n';
    std::cout << "negative test set statistics: " << '\n';
    std::cout << "average: " << negative_values.mean() << ", stddev: " << negative_values.stddev() << '\n'
              << "min: " << negative_values.min() << ", max: " << negative_values.max()  << '\n'
              << "skewness: " << negative_values.skewness() << ", excess kurtosis: " << negative_values.ex_kurtosis() << '\n';

    int num_correct = true_positives + true_negatives,
        num_wrong = false_positives + false_negatives;
    std::cout << "\n\n";
    std::cout << "true positives: " << true_positives << ", false positives: " << false_positives << '\n';
    std::cout << "true negatives: " << true_negatives << ", false negatives: " << false_negatives << '\n';
    std::cout << "number of samples classified corretly: " << num_correct << '\n';
    std::cout << "number of samples classified incorrectly: " << num_wrong << '\n';
    std::cout << "accuracy: " << num_correct/float(num_correct + num_wrong) << '\n';
    return 0;
}


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

#ifndef HOMENCIPHER_HPP
#define HOMENCIPHER_HPP

#include <NTL/ZZ.h>
#include "Encipher.hpp"
#include "Random.h"

/**
 * This abstract class details all the members and methods that are common to the HE\em x algorithms.
 * All of the HE\em x encryption classes derive from and implement this class.
 * @tparam C Ciphertext type
 * @tparam P Plaintext type
 */
template <typename C, typename P> class HOMEncipher : public Encipher<C,P>
{
protected:
	/**
	 * Modulus (=pq) for arithmetic operations on ciphertexts. A public parameter of the cipher.
	 */
	NTL::ZZ modulus;
	/**
	 * A secret parameter of the cipher.
	 */
	NTL::ZZ p;
	/**
	 * A secret parameter of the cipher.
	 */
	NTL::ZZ q;
	/**
	 * Secure random number generator.
	 */
	Random* rng;

	void generateModulus(int lambda, int eta){
		p = NTL::RandomPrime_ZZ(lambda,20);
		q = NTL::RandomPrime_ZZ(eta,20);
		modulus = p*q;
	}

public:
	/**
	 * Initialize the cipher by setting the modulus for arithmetic operations
	 */
	void init(){
		this->rng = new Random();
		NTL::ZZ_p::init(modulus);
	};

	/**
	 * Accessor to retrieve public modulus for arithmetic operations on ciphertexts
	 * @return Modulus
	 */
	NTL::ZZ getModulus(){
		return modulus;
	};

	/**
	 * Write the public cipher parameters to a JSON string
	 * @return JSON string
	 */
	virtual std::string writeParametersToJSON()=0;

	/**
	 * Default constructor
	 */
	HOMEncipher(){
		rng=nullptr;
	};

	/**
	 * Default destructor
	 */
	virtual ~HOMEncipher(){
		delete rng;
	};
};

#endif


#ifndef BOOST_METAPARSE_GETTING_STARTED_9_HPP
#define BOOST_METAPARSE_GETTING_STARTED_9_HPP

// Automatically generated header file

// Definitions before section 8.2
#include "8_2.hpp"

// Definitions of section 8.2
template <class S, class Item>
 struct reverse_binary_op :
   eval_binary_op<
     typename boost::mpl::at_c<Item, 0>::type,
     boost::mpl::at_c<Item, 1>::type::value,
     S
   >
   {};

#include <boost/metaparse/foldr_start_with_parser.hpp>

using mult_exp3 =
 foldr_start_with_parser<
   sequence<int_token, one_of<times_token, divides_token>>, /* The parser applied repeatedly */
   int_token, /* The parser parsing the last number */
   boost::mpl::quote2<reverse_binary_op> /* The function called for every result */
                                         /* of applying the above parser */
 >;

using exp_parser17 =
 build_parser<
   foldl_start_with_parser<
     sequence<one_of<plus_token, minus_token>, mult_exp3>,
     mult_exp3,
     boost::mpl::quote2<binary_op>
   >
 >;

// query:
//    exp_parser17::apply<BOOST_METAPARSE_STRING("8 / 4 / 2")>::type

#endif