// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is 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.
// __END_LICENSE__

#include <test/Helpers.h>
#include <vw/Image/EdgeExtension.h>
#include <vw/Image/UtilityViews.h>
#include <vw/Image/Algorithms.h>
#include <vw/Stereo/CostFunctions.h>
#include <vw/Stereo/Correlation.h>

#include <boost/random/linear_congruential.hpp>

using namespace vw;
using namespace vw::stereo;

template <typename  PixelT>
class Correlation : public ::testing::Test {
protected:
  Correlation(){}

  typedef PixelT            pixel_type;
  typedef ImageView<PixelT> image_type;
  typedef ImageView<PixelMask<Vector2i> >         result_type;
  typedef typename PixelChannelType<PixelT>::type channel_type;
  image_type input1, input2;
  Vector2i   kernel_size;
  Vector2i   search_volume;
  Vector2i   solution;

  virtual void SetUp() {
    boost::rand48 gen(10);
    kernel_size   = Vector2i(7,5);
    search_volume = Vector2i(7,12);
    solution      = Vector2i(3,8);
    input1 = pixel_cast_rescale<pixel_type>(uniform_noise_view(gen,25,25));
    input2 = crop( edge_extend( input1, ConstantEdgeExtension() ), -solution[0], -solution[1],
                   25+search_volume[0]-1, 35+search_volume[1]-1);
  }

  template <class ImageT>
  void CheckResult( ImageViewBase<ImageT> const& imagebase ) {
    ImageT const& image = imagebase.impl();
    for ( int32 i = 0; i < image.cols(); i++ ) {
      for ( int32 j = 0; j < image.rows(); j++ ) {
        EXPECT_TRUE( is_valid(image(i,j)) );
        EXPECT_VW_EQ( solution, image(i,j).child() );
      }
    }
  }
};

typedef Correlation<PixelGray<uint8> > CorrelationGRAYU8;
typedef Correlation<PixelGray<int16> > CorrelationGRAYI16;
typedef Correlation<PixelGray<float> > CorrelationGRAYF32;
typedef Correlation<uint8>             CorrelationU8;

TEST_F( CorrelationGRAYU8, AbsDifference ) {
  result_type disparity =
    calc_disparity( ABSOLUTE_DIFFERENCE,
                    input1, input2,
                    bounding_box( input1 ),
                    search_volume, kernel_size );
  ASSERT_EQ( 19, disparity.cols() );
  ASSERT_EQ( 21, disparity.rows() );
  ASSERT_TRUE( is_valid(disparity(10,10)) );
  CheckResult( disparity );
}

TEST_F( CorrelationGRAYU8, SquaredDifference ) {
  result_type disparity =
    calc_disparity( SQUARED_DIFFERENCE,
                    input1, input2,
                    bounding_box( input1 ),
                    search_volume, kernel_size );
  ASSERT_EQ( 19, disparity.cols() );
  ASSERT_EQ( 21, disparity.rows() );
  ASSERT_TRUE( is_valid(disparity(10,10)) );
  CheckResult( disparity );
}

TEST_F( CorrelationGRAYU8, CrossCorrelation ) {
  result_type disparity =
    calc_disparity( CROSS_CORRELATION,
                    input1, input2,
                    bounding_box( input1 ),
                    search_volume, kernel_size );
  ASSERT_EQ( 19, disparity.cols() );
  ASSERT_EQ( 21, disparity.rows() );
  ASSERT_TRUE( is_valid(disparity(10,10)) );
  CheckResult( disparity );
}

TEST_F( CorrelationGRAYI16, AbsDifference ) {
  result_type disparity =
    calc_disparity( ABSOLUTE_DIFFERENCE,
                    input1, input2,
                    bounding_box( input1 ),
                    search_volume, kernel_size );
  ASSERT_EQ( 19, disparity.cols() );
  ASSERT_EQ( 21, disparity.rows() );
  ASSERT_TRUE( is_valid(disparity(10,10)) );
  CheckResult( disparity );
}

TEST_F( CorrelationGRAYI16, SquaredDifference ) {
  result_type disparity =
    calc_disparity( SQUARED_DIFFERENCE,
                    input1, input2,
                    bounding_box( input1 ),
                    search_volume, kernel_size );
  ASSERT_EQ( 19, disparity.cols() );
  ASSERT_EQ( 21, disparity.rows() );
  ASSERT_TRUE( is_valid(disparity(10,10)) );
  CheckResult( disparity );
}

TEST_F( CorrelationGRAYI16, CrossCorrelation ) {
  result_type disparity =
    calc_disparity( CROSS_CORRELATION,
                    input1, input2,
                    bounding_box( input1 ),
                    search_volume, kernel_size );
  ASSERT_EQ( 19, disparity.cols() );
  ASSERT_EQ( 21, disparity.rows() );
  ASSERT_TRUE( is_valid(disparity(10,10)) );
  CheckResult( disparity );
}

TEST_F( CorrelationGRAYF32, AbsDifference ) {
  result_type disparity =
    calc_disparity( ABSOLUTE_DIFFERENCE,
                    input1, input2,
                    bounding_box( input1 ),
                    search_volume, kernel_size );
  ASSERT_EQ( 19, disparity.cols() );
  ASSERT_EQ( 21, disparity.rows() );
  ASSERT_TRUE( is_valid(disparity(10,10)) );
  CheckResult( disparity );
}

TEST_F( CorrelationGRAYF32, SquaredDifference ) {
  result_type disparity =
    calc_disparity( SQUARED_DIFFERENCE,
                    input1, input2,
                    bounding_box( input1 ),
                    search_volume, kernel_size );
  ASSERT_EQ( 19, disparity.cols() );
  ASSERT_EQ( 21, disparity.rows() );
  ASSERT_TRUE( is_valid(disparity(10,10)) );
  CheckResult( disparity );
}

TEST_F( CorrelationGRAYF32, CrossCorrelation ) {
  result_type disparity =
    calc_disparity( CROSS_CORRELATION,
                    input1, input2,
                    bounding_box( input1 ),
                    search_volume, kernel_size );
  ASSERT_EQ( 19, disparity.cols() );
  ASSERT_EQ( 21, disparity.rows() );
  ASSERT_TRUE( is_valid(disparity(10,10)) );
  CheckResult( disparity );
}

TEST_F( CorrelationU8, AbsDifference ) {
  result_type disparity =
    calc_disparity( ABSOLUTE_DIFFERENCE,
                    input1, input2,
                    bounding_box( input1 ),
                    search_volume, kernel_size );
  ASSERT_EQ( 19, disparity.cols() );
  ASSERT_EQ( 21, disparity.rows() );
  ASSERT_TRUE( is_valid(disparity(10,10)) );
  CheckResult( disparity );
}

TEST_F( CorrelationU8, SquaredDifference ) {
  result_type disparity =
    calc_disparity( SQUARED_DIFFERENCE,
                    input1, input2,
                    bounding_box( input1 ),
                    search_volume, kernel_size );
  ASSERT_EQ( 19, disparity.cols() );
  ASSERT_EQ( 21, disparity.rows() );
  ASSERT_TRUE( is_valid(disparity(10,10)) );
  CheckResult( disparity );
}

TEST_F( CorrelationU8, CrossCorrelation ) {
  result_type disparity =
    calc_disparity( CROSS_CORRELATION,
                    input1, input2,
                    bounding_box( input1 ),
                    search_volume, kernel_size );
  ASSERT_EQ( 19, disparity.cols() );
  ASSERT_EQ( 21, disparity.rows() );
  ASSERT_TRUE( is_valid(disparity(10,10)) );
  CheckResult( disparity );
}


/*
 * The information in this file is
 * Copyright(c) 2010 Trevor R.H. Clarke
 * and is subject to the terms and conditions of the
 * GNU Lesser General Public License Version 2.1
 * The license text is available from
 * http://www.gnu.org/licenses/lgpl.html
 */

#include "AppConfig.h"
#include "LocationType.h"
#include "RasterData.h"
#include "TrackingUtils.h"
#include <stdlib.h>
#include <opencv/cv.h>

#if defined(WIN_API)
#include <boost/cstdint.hpp>
using boost::uint8_t;
#endif

namespace TrackingUtils
{
subcubeid_t calculateSubcubeId(LocationType location, uint8_t levels,
                               Opticks::PixelLocation maxBb,
                               Opticks::PixelLocation minBb)
{
   subcubeid_t id(levels);
   for (uint8_t level = 0; level < levels; ++level)
   {
      Opticks::PixelLocation center(static_cast<int>((maxBb.mX - minBb.mX + 1) / 2.0 + minBb.mX),
                                    static_cast<int>((maxBb.mY - minBb.mY + 1) / 2.0 + minBb.mY));
      if (location.mX >= center.mX && location.mY <= center.mY) // quadrant 0
      {
         minBb.mX = center.mX;
         maxBb.mY = center.mY;
      }
      else if (location.mX < center.mX && location.mY <= center.mY) // quadrant 1
      {
         id |= 1 << (level * 2 + 4);
         maxBb = center;
      }
      else if (location.mX < center.mX && location.mY > center.mY) // quadrant 2
      {
         id |= 2 << (level * 2 + 4);
         maxBb.mX = center.mX;
         minBb.mY = center.mY;
      }
      else // quadrant 3
      {
         id |= 3 << (level * 2 + 4);
         minBb = center;
      }
   }
   return id;
}

bool calculateSubcubeBounds(subcubeid_t subcubeId, uint8_t& levels, Opticks::PixelLocation& maxBb, Opticks::PixelLocation& minBb)
{
   levels = subcubeId & 0x0f;
   subcubeId >>= 4;
   for (uint8_t level = 0; level < levels; ++level)
   {
      Opticks::PixelLocation center(static_cast<int>((maxBb.mX - minBb.mX + 1) / 2.0 + minBb.mX),
                                    static_cast<int>((maxBb.mY - minBb.mY + 1) / 2.0 + minBb.mY));
      switch (subcubeId & 0x03) // quadrant number
      {
      case 0:
         minBb.mX = center.mX;
         maxBb.mY = center.mY;
         break;
      case 1:
         maxBb = center;
         break;
      case 2:
         maxBb.mX = center.mX;
         minBb.mY = center.mY;
         break;
      case 3:
         minBb = center;
         break;
      default:
         break; // unreachable
      }
      subcubeId >>= 2;
   }
   return subcubeId == 0;
}

uint8_t calculateNeededLevels(uint32_t maxSubcubeSize,
                              Opticks::PixelLocation maxBb,
                              Opticks::PixelLocation minBb)
{
   maxBb = (maxBb - minBb) + 1;
   for (uint8_t level = 0; level <= 12; ++level) // maximum number of levels which can be represented
   {
      uint32_t curSize = maxBb.mX * maxBb.mY;
      if (curSize <= maxSubcubeSize)
      {
         return level;
      }
      maxBb.mX /= 2;
      maxBb.mY /= 2;
   }
   return 0xff; // Invalid return
}

CvMat* ransac_affine(std::vector<std::pair<CvPoint2D32f, CvPoint2D32f> >& corr, int maxIter, float thresh, unsigned int numNeeded)
{
   if (corr.empty())
   {
      return NULL;
   }
   thresh = thresh * thresh; // calculating sqrt for each test is expensive, so we square the check value to change from RMS to MSE
   unsigned int bestCount = 0;
   CvMat* pBest = NULL;
   CvMat* pTest = cvCreateMat(2, 3, CV_32F);
   CvMat* pVer = cvCreateMat(3, 1, CV_32F);
   CvMat* pVerRes = cvCreateMat(2, 1, CV_32F);
   pVer->data.fl[2] = 1.0;
   for (int it = 0; it < maxIter; ++it)
   {
      // build mss
      int pMss[3];
      pMss[0] = rand() % corr.size();
      do
      {
         pMss[1] = rand() % corr.size();
      }
      while (pMss[1] == pMss[0]);
      do
      {
         pMss[2] = rand() % corr.size();
      }
      while (pMss[2] == pMss[0] || pMss[2] == pMss[1]);
      CvPoint2D32f pSrc[3] = {corr[pMss[0]].first, corr[pMss[1]].first, corr[pMss[2]].first};
      CvPoint2D32f pDst[3] = {corr[pMss[0]].second, corr[pMss[1]].second, corr[pMss[2]].second};

      cvGetAffineTransform(pSrc, pDst, pTest);

      // test the mss
      unsigned int fitCount = 0;
      for (unsigned int j = 0; j < corr.size(); ++j)
      {
         if (j == pMss[0] || j == pMss[1] || j == pMss[2])
         {
            continue;
         }
         pVer->data.fl[0] = corr[j].first.x;
         pVer->data.fl[1] = corr[j].first.y;
         cvGEMM(pTest, pVer, 1.0, NULL, 0.0, pVerRes);
         float tmpX = pVerRes->data.fl[0] - corr[j].second.x;
         float tmpY = pVerRes->data.fl[1] - corr[j].second.y;
         float mse = (tmpX * tmpX + tmpY * tmpY) / 2.0f;
         if (mse < thresh)
         {
            // fits the affine transform
            fitCount++;
         }
      }
      if (fitCount >= numNeeded && fitCount > bestCount)
      {
         if (pBest == NULL)
         {
            pBest = pTest;
            pTest = cvCreateMat(2, 3, CV_32F);
         }
         else
         {
            std::swap(pBest, pTest);
         }
         bestCount = fitCount;
      }
   }
   cvReleaseMat(&pTest);
   cvReleaseMat(&pVer);
   cvReleaseMat(&pVerRes);
   return pBest;
}
}

IplImageResource::IplImageResource() : mpImage(NULL), mShallow(false)
{
}

IplImageResource::IplImageResource(IplImage* pImage) : mpImage(pImage), mShallow(false)
{
}

IplImageResource::IplImageResource(const IplImageResource& other)
{
   reset(const_cast<IplImage*>(other.get()));
   if (other.isShallow())
   {
      release();
   }
   else
   {
      take();
   }
   const_cast<IplImageResource&>(other).mpImage = NULL;
}

IplImageResource::IplImageResource(int width, int height, int depth, int channels) : mShallow(false)
{
   mpImage = cvCreateImage(cvSize(width, height), depth, channels);
}

IplImageResource::IplImageResource(int width, int height, int depth, int channels, char* pData) : mShallow(true)
{
   mpImage = cvCreateImageHeader(cvSize(width, height), depth, channels);
   mpImage->imageData = pData;
}

IplImageResource::~IplImageResource()
{
   reset(NULL);
}

bool IplImageResource::isShallow() const
{
   return mShallow;
}

void IplImageResource::reset(IplImage* pImage)
{
   if (mpImage != NULL)
   {
      if (mShallow)
      {
         mpImage->imageData = NULL;
      }
      cvReleaseImage(&mpImage);
   }
   mpImage = pImage;
   mShallow = false;
}

IplImage* IplImageResource::get()
{
   return mpImage;
}

const IplImage* IplImageResource::get() const
{
   return mpImage;
}

IplImage* IplImageResource::release()
{
   mShallow = true;
   return mpImage;
}

IplImage* IplImageResource::take()
{
   mShallow = false;
   return mpImage;
}

IplImageResource::operator IplImage*()
{
   return get();
}

IplImage& IplImageResource::operator*()
{
   return *get();
}

IplImageResource& IplImageResource::operator=(const IplImageResource& other)
{
   mShallow = other.mShallow;
   mpImage = other.mpImage;
   const_cast<IplImageResource&>(other).mShallow = true;
   const_cast<IplImageResource&>(other).mpImage = NULL;
   return *this;
}

dataptr::dataptr(DataElement* pElement, DataPointerArgs args)
{
   int own(0);
   mpData = createDataPointer(pElement, &args, &own);
   mOwns = (own != 0);
}

dataptr::~dataptr()
{
   if (mOwns)
   {
      destroyDataPointer(mpData);
   }
}

dataptr::operator void*()
{
   return mpData;
}

dataptr::operator char*()
{
   return reinterpret_cast<char*>(mpData);
}

void* dataptr::get()
{
   return mpData;
}


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-predicates
    Function object implementing is_less_equal capabilities

    Returns @ref True  or @ref False depending on whether x is
    less than or equal to y or not.

    Infix notation can be used with operator '<='.

    @par Semantic:

    @code
    auto r = is_less_equal(x,y);
    @endcode

    or

    @code
    auto r = x <= y;
    @endcode

    @see  is_equal, is_eqz

  **/
  as_logical_t<Value> is_less_equal(Value const& x, Value const& y);
} }
#endif

#include <boost/simd/function/scalar/is_less_equal.hpp>
#include <boost/simd/function/simd/is_less_equal.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
 *      110620    F.M. Engelen      File created.
 *      110721    J. Melman         Comments, variable names, and consistency modified.
 *      110722    F.M. Engelen      Removed setRelativePath function.
 *
 *    References
 *
 *    Notes
 *
 */

#include <iostream>

#include <boost/make_shared.hpp>

#include "Tudat/InputOutput/matrixTextFileReader.h"

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

namespace tudat
{
namespace aerodynamics
{

//! Initialize atmosphere table reader.
void TabulatedAtmosphere::initialize( const std::string& atmosphereTableFile )
{
    // Locally store the atmosphere table file name.
    atmosphereTableFile_ = atmosphereTableFile;

    Eigen::MatrixXd containerOfAtmosphereTableFileData
            = input_output::readMatrixFromFile( atmosphereTableFile_, " \t", "%" );

    // Check whether data is present in the file.
    if ( containerOfAtmosphereTableFileData.rows( ) < 1
         || containerOfAtmosphereTableFileData.cols( ) < 1 )
    {
        std::cerr << "The atmosphere table file is empty." << std::endl;
        std::cerr << atmosphereTableFile_ << std::endl;
    }

    // Initialize vectors.
    altitudeData_.resize( containerOfAtmosphereTableFileData.rows( ) );
    densityData_.resize( containerOfAtmosphereTableFileData.rows( ) );
    pressureData_.resize( containerOfAtmosphereTableFileData.rows( ) );
    temperatureData_.resize( containerOfAtmosphereTableFileData.rows( ) );

    // Loop through all the strings stored in the container and store the data
    // in the right Eigen::VectorXd.
    for ( int i = 0; i < containerOfAtmosphereTableFileData.rows( ); i++  )
    {
        altitudeData_[ i ] = containerOfAtmosphereTableFileData( i, 0 );
        densityData_[ i ] = containerOfAtmosphereTableFileData( i, 1 );
        pressureData_[ i ] = containerOfAtmosphereTableFileData( i, 2 );
        temperatureData_[ i ] = containerOfAtmosphereTableFileData( i, 3 );
    }

    using namespace interpolators;

    cubicSplineInterpolationForDensity_
            = boost::make_shared< CubicSplineInterpolatorDouble >( altitudeData_, densityData_ );
    cubicSplineInterpolationForPressure_
            = boost::make_shared< CubicSplineInterpolatorDouble >( altitudeData_, pressureData_ );
    cubicSplineInterpolationForTemperature_
            = boost::make_shared< CubicSplineInterpolatorDouble >(
                altitudeData_, temperatureData_ );
}

} // namespace aerodynamics
} // namespace tudat


#include <iostream>
#include <iomanip>
#include <array>
#include <vector>
#include <deque>
#include <forward_list>
#include <list>
#include <map>
#include <set>
#include <unordered_map>
#include <unordered_set>
#include <string>
#include <Eigen/Geometry>
#include <arc_utilities/arc_helpers.hpp>

#ifndef PRETTY_PRINT_HPP
#define PRETTY_PRINT_HPP

// Handy functions for printing vectors and pairs
namespace PrettyPrint
{
    // Base template function for printing types
    template <typename T>
    inline std::string PrettyPrint(const T& toprint, const bool add_delimiters=false, const std::string& separator=", ")
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        std::ostringstream strm;
        strm << toprint;
        return strm.str();
    }

    /////////////////////////////////////////////////////////////////////////////////////////////////////////
    /////                                       PROTOTYPES ONLY                                         /////
    ///// Specializations for specific types - if you want a specialization for a new type, add it here /////
    /////////////////////////////////////////////////////////////////////////////////////////////////////////

    template<>
    inline std::string PrettyPrint(const bool& bool_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::Vector2d& vector_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::Vector3d& vector_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::Vector4d& vector_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::VectorXd& vector_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::MatrixXd& matrix_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::Quaterniond& quaternion_to_print, const bool add_delimiters, const std::string& separator);

    template<>
    inline std::string PrettyPrint(const Eigen::Isometry3d& transform_to_print, const bool add_delimiters, const std::string& separator);

    template <typename A, typename B>
    inline std::string PrettyPrint(const std::pair<A, B>& pairtoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, size_t N>
    inline std::string PrettyPrint(const std::array<T, N>& arraytoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::vector<T, Allocator>& vectoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::list<T, Allocator>& listtoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::forward_list<T, Allocator>& listtoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::deque<T, Allocator>& dequetoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename A, typename B, typename Compare=std::less<A>, typename Allocator=std::allocator<std::pair<const A, B>>>
    inline std::string PrettyPrint(const std::map<A, B, Compare, Allocator>& maptoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename A, typename B, typename Compare=std::less<A>, typename Allocator=std::allocator<std::pair<const A, B>>>
    inline std::string PrettyPrint(const std::multimap<A, B, Compare, Allocator>& maptoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Compare=std::less<T>, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::set<T, Compare, Allocator>& settoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Compare=std::less<T>, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::multiset<T, Compare, Allocator>& settoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename A, typename B, typename Hash=std::hash<A>, typename Predicate=std::equal_to<A>, typename Allocator=std::allocator<std::pair<const A, B>>>
    inline std::string PrettyPrint(const std::unordered_map<A, B, Hash, Predicate, Allocator>& maptoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename A, typename B, typename Hash=std::hash<A>, typename Predicate=std::equal_to<A>, typename Allocator=std::allocator<std::pair<const A, B>>>
    inline std::string PrettyPrint(const std::unordered_multimap<A, B, Hash, Predicate, Allocator>& maptoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Hash=std::hash<T>, typename Predicate=std::equal_to<T>, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::unordered_set<T, Hash, Predicate, Allocator>& settoprint, const bool add_delimiters=false, const std::string& separator=", ");

    template <typename T, typename Hash=std::hash<T>, typename Predicate=std::equal_to<T>, typename Allocator=std::allocator<T>>
    inline std::string PrettyPrint(const std::unordered_multiset<T, Hash, Predicate, Allocator>& settoprint, const bool add_delimiters=false, const std::string& separator=", ");

    /////////////////////////////////////////////////////////////////////////////////////////////////////////
    /////                                   IMPLEMENTATIONS ONLY                                        /////
    ///// Specializations for specific types - if you want a specialization for a new type, add it here /////
    /////////////////////////////////////////////////////////////////////////////////////////////////////////

    template<>
    inline std::string PrettyPrint(const bool& bool_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        if (bool_to_print)
        {
            return "true";
        }
        else
        {
            return "false";
        }
    }

    template<>
    inline std::string PrettyPrint(const Eigen::Vector2d& vector_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        std::ostringstream strm;
        strm << std::setprecision(12) << "Vector2d: <x: " << vector_to_print(0) << " y: " << vector_to_print(1) << ">";
        return strm.str();
    }

    template<>
    inline std::string PrettyPrint(const Eigen::Vector3d& vector_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        std::ostringstream strm;
        strm << std::setprecision(12) << "Vector3d: <x: " << vector_to_print.x() << " y: " << vector_to_print.y() << " z: " << vector_to_print.z() << ">";
        return strm.str();
    }

    template<>
    inline std::string PrettyPrint(const Eigen::Vector4d& vector_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        std::ostringstream strm;
        strm << std::setprecision(12) << "Vector4d: <x: " << vector_to_print(0) << " y: " << vector_to_print(1) << " z: " << vector_to_print(2) << " w: " << vector_to_print(3) << ">";
        return strm.str();
    }

    template<>
    inline std::string PrettyPrint(const Eigen::VectorXd& vector_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        if (vector_to_print.size() > 1)
        {
            std::ostringstream strm;
            strm << "VectorXd: <" << std::to_string(vector_to_print(0));
            for (std::ptrdiff_t idx = 1; idx < vector_to_print.size(); idx++)
            {
                strm << ", " << std::to_string(vector_to_print(idx));
            }
            strm << ">";
            return strm.str();
        }
        else if (vector_to_print.size() == 1)
        {
            return "VectorXd: <" + std::to_string(vector_to_print(0)) + ">";
        }
        else
        {
            return "VectorXd: <>";
        }
    }

    template<>
    inline std::string PrettyPrint(const Eigen::MatrixXd& matrix_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        if (matrix_to_print.rows() > 0 && matrix_to_print.cols() > 0)
        {
            std::ostringstream strm;
            strm << "MatrixXd:\n[";
            strm << std::to_string(matrix_to_print(0, 0));
            for (int64_t col = 1; col < matrix_to_print.cols(); col++)
            {
                strm << ", " << std::to_string(matrix_to_print(0, col));
            }
            for (int64_t row = 1; row < matrix_to_print.rows(); row++)
            {
                strm << "\n" << std::to_string(matrix_to_print(row, 0));
                for (int64_t col = 1; col < matrix_to_print.cols(); col++)
                {
                    strm << ", " << std::to_string(matrix_to_print(row, col));
                }
            }
            strm << "]";
            return strm.str();
        }
        else
        {
            return "MatrixXd:\n[]";
        }
    }

    template<>
    inline std::string PrettyPrint(const Eigen::Quaterniond& quaternion_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        return "Quaterniond <x: " + std::to_string(quaternion_to_print.x()) + " y: " + std::to_string(quaternion_to_print.y()) + " z: " + std::to_string(quaternion_to_print.z()) + " w: " + std::to_string(quaternion_to_print.w()) + ">";
    }

    template<>
    inline std::string PrettyPrint(const Eigen::Isometry3d& transform_to_print, const bool add_delimiters, const std::string& separator)
    {
        UNUSED(add_delimiters);
        UNUSED(separator);
        Eigen::Vector3d vector_to_print = transform_to_print.translation();
        Eigen::Quaterniond quaternion_to_print(transform_to_print.rotation());
        return "Isometry3d <x: " + std::to_string(vector_to_print.x()) + " y: " + std::to_string(vector_to_print.y()) + " z: " + std::to_string(vector_to_print.z()) + ">, <x: " + std::to_string(quaternion_to_print.x()) + " y: " + std::to_string(quaternion_to_print.y()) + " z: " + std::to_string(quaternion_to_print.z()) + " w: " + std::to_string(quaternion_to_print.w()) + ">";
    }

    template <typename A, typename B>
    inline std::string PrettyPrint(const std::pair<A, B>& pairtoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (add_delimiters)
        {
            strm << "<" << PrettyPrint(pairtoprint.first, add_delimiters, separator) << ": " << PrettyPrint(pairtoprint.second, add_delimiters, separator) << ">";
        }
        else
        {
            strm << PrettyPrint(pairtoprint.first, add_delimiters, separator) << ": " << PrettyPrint(pairtoprint.second, add_delimiters, separator);
        }
        return strm.str();
    }

    template <typename T, size_t N>
    inline std::string PrettyPrint(const std::array<T, N>& arraytoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (arraytoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "[" << PrettyPrint(arraytoprint[0], add_delimiters, separator);
                for (size_t idx = 1; idx < arraytoprint.size(); idx++)
                {
                    strm << separator << PrettyPrint(arraytoprint[idx], add_delimiters, separator);
                }
                strm << "]";
            }
            else
            {
                strm << PrettyPrint(arraytoprint[0], add_delimiters, separator);
                for (size_t idx = 1; idx < arraytoprint.size(); idx++)
                {
                    strm << separator << PrettyPrint(arraytoprint[idx], add_delimiters, separator);
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Allocator>
    inline std::string PrettyPrint(const std::vector<T, Allocator>& vectoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (vectoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "[" << PrettyPrint(vectoprint[0], add_delimiters, separator);
                for (size_t idx = 1; idx < vectoprint.size(); idx++)
                {
                    strm << separator << PrettyPrint(vectoprint[idx], add_delimiters, separator);
                }
                strm << "]";
            }
            else
            {
                strm << PrettyPrint(vectoprint[0], add_delimiters, separator);
                for (size_t idx = 1; idx < vectoprint.size(); idx++)
                {
                    strm << separator << PrettyPrint(vectoprint[idx], add_delimiters, separator);
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Allocator>
    inline std::string PrettyPrint(const std::list<T, Allocator>& listtoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (listtoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "[";
                typename std::list<T, Allocator>::const_iterator itr;
                for (itr = listtoprint.begin(); itr != listtoprint.end(); ++itr)
                {
                    if (itr != listtoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << "]";
            }
            else
            {
                typename std::list<T, Allocator>::const_iterator itr;
                for (itr = listtoprint.begin(); itr != listtoprint.end(); ++itr)
                {
                    if (itr != listtoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Allocator>
    inline std::string PrettyPrint(const std::forward_list<T, Allocator>& listtoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (listtoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "[";
                typename std::forward_list<T, Allocator>::const_iterator itr;
                for (itr = listtoprint.begin(); itr != listtoprint.end(); ++itr)
                {
                    if (itr != listtoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << "]";
            }
            else
            {
                typename std::forward_list<T, Allocator>::const_iterator itr;
                for (itr = listtoprint.begin(); itr != listtoprint.end(); ++itr)
                {
                    if (itr != listtoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Allocator>
    inline std::string PrettyPrint(const std::deque<T, Allocator>& dequetoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (dequetoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "[";
                typename std::deque<T, Allocator>::const_iterator itr;
                for (itr = dequetoprint.begin(); itr != dequetoprint.end(); ++itr)
                {
                    if (itr != dequetoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << "]";
            }
            else
            {
                typename std::deque<T, Allocator>::const_iterator itr;
                for (itr = dequetoprint.begin(); itr != dequetoprint.end(); ++itr)
                {
                    if (itr != dequetoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename A, typename B, typename Compare, typename Allocator>
    inline std::string PrettyPrint(const std::map<A, B, Compare, Allocator>& maptoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (maptoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "{";
                typename std::map<A, B, Compare, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
                strm << "}";
            }
            else
            {
                typename std::map<A, B, Compare, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename A, typename B, typename Compare, typename Allocator>
    inline std::string PrettyPrint(const std::multimap<A, B, Compare, Allocator>& maptoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (maptoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "{";
                typename std::multimap<A, B, Compare, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
                strm << "}";
            }
            else
            {
                typename std::multimap<A, B, Compare, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Compare, typename Allocator>
    inline std::string PrettyPrint(const std::set<T, Compare, Allocator>& settoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (settoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "(";
                typename std::set<T, Compare, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << ")";
            }
            else
            {
                typename std::set<T, Compare, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Compare, typename Allocator>
    inline std::string PrettyPrint(const std::multiset<T, Compare, Allocator>& settoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (settoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "(";
                typename std::multiset<T, Compare, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << ")";
            }
            else
            {
                typename std::multiset<T, Compare, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename A, typename B, typename Hash, typename Predicate, typename Allocator>
    inline std::string PrettyPrint(const std::unordered_map<A, B, Hash, Predicate, Allocator>& maptoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (maptoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "{";
                typename std::unordered_map<A, B, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
                strm << "}";
            }
            else
            {
                typename std::unordered_map<A, B, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename A, typename B, typename Hash, typename Predicate, typename Allocator>
    inline std::string PrettyPrint(const std::unordered_multimap<A, B, Hash, Predicate, Allocator>& maptoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (maptoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "{";
                typename std::unordered_multimap<A, B, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
                strm << "}";
            }
            else
            {
                typename std::unordered_multimap<A, B, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = maptoprint.begin(); itr != maptoprint.end(); ++itr)
                {
                    std::pair<A, B> cur_pair(itr->first, itr->second);
                    if (itr != maptoprint.begin())
                    {
                        strm << separator << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(cur_pair, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Hash, typename Predicate, typename Allocator>
    inline std::string PrettyPrint(const std::unordered_set<T, Hash, Predicate, Allocator>& settoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (settoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "(";
                typename std::unordered_set<T, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << ")";
            }
            else
            {
                typename std::unordered_set<T, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }

    template <typename T, typename Hash, typename Predicate, typename Allocator>
    inline std::string PrettyPrint(const std::unordered_multiset<T, Hash, Predicate, Allocator>& settoprint, const bool add_delimiters, const std::string& separator)
    {
        std::ostringstream strm;
        if (settoprint.size() > 0)
        {
            if (add_delimiters)
            {
                strm << "(";
                typename std::unordered_multiset<T, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
                strm << ")";
            }
            else
            {
                typename std::unordered_multiset<T, Hash, Predicate, Allocator>::const_iterator itr;
                for (itr = settoprint.begin(); itr != settoprint.end(); ++itr)
                {
                    if (itr != settoprint.begin())
                    {
                        strm << separator << PrettyPrint(*itr, add_delimiters, separator);
                    }
                    else
                    {
                        strm << PrettyPrint(*itr, add_delimiters, separator);
                    }
                }
            }
        }
        return strm.str();
    }
}

#endif // PRETTY_PRINT_HPP


#include "algorithm-utils-cpp/algorithm-utils.h"

#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH

#include <iostream>

bool is_even(const int& number)
{
    return number % 2 == 0;
}

bool is_greater_than(int base, const int& number)
{
    return number > base;
}

bool c_is_greater_than_a_plus_b(int a, int b, const int& c)
{
    return c > a + b;
}

void test_item_exists()
{
    std::vector<int> numbers;

    numbers.push_back(1);
    assert(!item_exists<int>(numbers, is_even));

    numbers.push_back(2);
    assert(item_exists<int>(numbers, is_even));

    std::cout << "test_item_exists: OK" << std::endl;
}

void test_remove_items_if()
{
    std::vector<int> numbers;
    for (int i = 1; i < 10; ++i)
        numbers.push_back(i);

    remove_items_if<int>(numbers, is_even);

    assert(numbers.size() == 5);
    assert(numbers[1] == 3);

    std::cout << "test_remove_items_if: OK" << std::endl;
}

void test_copy_items_if()
{
    std::vector<int> numbers;
    for (int i = 1; i < 10; ++i)
        numbers.push_back(i);

    std::vector<int> even = copy_items_if<int>(numbers, is_even);

    assert(even.size() == 4);
    assert(even[1] == 4);

    // Example of passing a curried function that binds only the first
    // parameter, but leaves the second open
    std::vector<int> greater_than_6 =
        copy_items_if<int>(numbers, boost::bind(is_greater_than, 6, _1));

    assert(greater_than_6.size() == 3);
    assert(greater_than_6[0] == 7);

    // Example of negating criteria
    std::vector<int> odd =
        copy_items_if<int>(numbers, !boost::bind(is_even, _1));

    assert(odd.size() == 5);
    assert(odd[1] == 3);

    // Example of bind more than one parameter in different order
    std::vector<int> greater_than_7 = copy_items_if<int>(
        numbers, boost::bind(c_is_greater_than_a_plus_b, 3, 4, _1));

    assert(greater_than_7.size() == 2);
    assert(greater_than_7[0] == 8);

    // Example of leaving first parameter open and binding latter
    std::vector<int> less_than_4 = copy_items_if<int>(
        numbers, boost::bind(c_is_greater_than_a_plus_b, _1, 1, 5));

    assert(less_than_4.size() == 3);
    assert(less_than_4[2] == 3);

    // Example of negating bind
    std::vector<int> not_less_than_5 = copy_items_if<int>(
        numbers, !boost::bind(is_greater_than, _1, 5));

    assert(not_less_than_5.size() == 5);
    assert(not_less_than_5[4] == 9);

    std::cout << "test_copy_items_if: OK" << std::endl;
}

void test_find_item()
{
    std::vector<int> numbers;
    for (int i = 1; i < 10; ++i)
        numbers.push_back(i);

    std::vector<int>::iterator found = find_item<int>(numbers, is_even);

    assert(*found == 2);
    assert(numbers[1] == *found);

    std::cout << "test_find_item: OK" << std::endl;
}

class Item
{
public:
    Item(int number) : _number(number) {}
    bool is_even() const { return _number % 2 == 0; }
    bool is_greater(int other) const { return _number > other; }
    int number() const { return _number; }

private:
    int _number;
};

void test_objects()
{
    std::vector<Item> items;
    for (int i = 1; i < 10; ++i)
        items.push_back(Item(i));

    std::vector<Item> even = copy_items_if<Item>(items, &Item::is_even);

    assert(even.size() == 4);
    assert(even[1].number() == 4);

    std::vector<Item> greater_than_3 =
        copy_items_if<Item>(items, boost::bind(&Item::is_greater, _1, 3));

    assert(greater_than_3.size() == 6);
    assert(greater_than_3[1].number() == 5);

    std::vector<Item> only_7 =
        copy_items_if<Item>(items, boost::bind(&Item::number, _1) == 7);

    assert(only_7.size() == 1);
    assert(only_7[0].number() == 7);

    std::cout << "test_objects: OK" << std::endl;
}

int main()
{
    test_item_exists();
    test_remove_items_if();
    test_copy_items_if();
    test_find_item();
    test_objects();

    return 0;
}


#include <algorithm>
#include <cstdlib>
#include <fstream>
#include <future>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <type_traits>
#include <unordered_set>
#include <utility>
#include <vector>

#include <boost/hana/for_each.hpp>

#include <libkameris/distances.hpp>
#include <libkameris/utils.hpp>
#include <kameris-formats/cpp/dist_writer.hpp>
#include <kameris-formats/cpp/repr_reader.hpp>

#include "common/progress_bar.hpp"
#include "common/version.hpp"

using namespace std;
using namespace kameris;

const unsigned num_threads = thread::hardware_concurrency();

template <typename dist_t>
SymmetricDistanceMatrixAdapter<dist_t> make_distance_matrix(size_t size) {
	auto data = new dist_t[size * (size - 1) / 2];
	return make_symmetric_distance_adapter(data, size);
}

int main(int argc, char *argv[]) {
	cout << "This is generation_dists (from kameris-backend), " VERSION_LONG "." << endl << endl;
	if (argc != 4) {
		cout << "Usage: generation_dists <input file> <output file prefix> [manhat,info]" << endl;
		return 1;
	}

	string output_prefix = argv[2];
	string dists_to_do = argv[3];
	bool do_manhat = dists_to_do.find("manhat") != string::npos;
	bool do_info = dists_to_do.find("info") != string::npos;

	repr_reader reader(argv[1]);
	const repr_header &input_header = reader.header();
	if (input_header.is_sparse) {
		cout << "Sparse matrices are not supported" << endl;
		return 1;
	}

	size_t num_matrices = input_header.count;
	cout << num_matrices << " matrices, using " << num_threads << " threads" << endl << endl;

	vector<unique_ptr<decltype(reader.read_matrix(0))>> matrices;
	{
		ProgressBar bar(num_matrices);
		for (size_t i = 0; i < num_matrices; ++i) {
			matrices.push_back(make_unique<decltype(reader.read_matrix(0))>(reader.read_matrix(i)));
			bar.increment();
		}
	}

	ProgressBar bar(num_matrices * (num_matrices - 1) / 2);
	ParallelExecutor exec;
	vector<future<void>> results;

	auto manhat_dists = make_distance_matrix<uint32_t>(do_manhat ? num_matrices : 0);
	auto info_dists = make_distance_matrix<float>(do_info ? num_matrices : 0);

	for (size_t i = 0; i < num_matrices; ++i) {
		boost::hana::for_each(element_type_types, [&](auto type) {
			using Value = typename decltype(type)::type;

			if (element_type_for_type<Value> == input_header.value_type) {
				results.push_back(exec.enqueue([&, i]([[gnu::unused]] unsigned _ignore) {
					for (size_t j = i + 1; j < num_matrices; ++j) {
						if (do_manhat) {
							manhat_dists(i, j) = manhattan<uint32_t>(
								get<MatrixAdapter<Value>>(*matrices[i]), get<MatrixAdapter<Value>>(*matrices[j]));
						}
						if (do_info) {
							info_dists(i, j) = approx_info_dist<float>(
								get<MatrixAdapter<Value>>(*matrices[i]), get<MatrixAdapter<Value>>(*matrices[j]));
						}
					}
				}));
			}
		});
	}

	exec.execute(thread::hardware_concurrency());

	vector<unique_ptr<dist_writer>> dist_writers(2);
	if (do_manhat) {
		dist_writers[0] = make_unique<dist_writer>(
			output_prefix + "-manhat.mm-dist", dist_header{element_type::uint32, num_matrices});
	}
	if (do_info) {
		dist_writers[1] =
			make_unique<dist_writer>(output_prefix + "-info.mm-dist", dist_header{element_type::float32, num_matrices});
	}

	for (size_t i = 0; i < num_matrices; ++i) {
		results[i].wait();
		if (do_manhat) {
			dist_writers[0]->write_next_row(manhat_dists);
		}
		if (do_info) {
			dist_writers[1]->write_next_row(info_dists);
		}
		bar.increment(num_matrices - i - 1);
	}

	return 0;
}


//---------------------------------------------------------------------------//
//!
//! \file   Utility_TwoDGridGenerator.hpp
//! \author Alex Robinson
//! \brief  Two-dimensional grid generator class declaration
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_TWO_D_GRID_GENERATOR_HPP
#define UTILITY_TWO_D_GRID_GENERATOR_HPP

// Std Lib Includes
#include <functional>
#include <iostream>

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "Utility_TwoDInterpolationPolicy.hpp"
#include "Utility_GridGenerator.hpp"
#include "Utility_Vector.hpp"
#include "Utility_Deque.hpp"

namespace Utility{

//! The two-dimensional grid generator class
template<typename TwoDInterpPolicy>
class TwoDGridGenerator
{

public:

  //! Constructor
  TwoDGridGenerator( const double convergence_tol = 0.001,
                     const double absolute_diff_tol = 1e-12,
                     const double distance_tol = 1e-14 );

  //! Destructor
  virtual ~TwoDGridGenerator()
  { /* ... */ }

  //! Set verbose mode to on
  void setVerboseModeOn();

  //! Set verbose mode to off (default)
  void setVerboseModeOff();

  //! Check if verbose mode is on
  bool isVerboseModeOn() const;

  //! Throw exception on dirty convergence
  void throwExceptionOnDirtyConvergence();

  //! Warn on dirty convergence (default)
  void warnOnDirtyConvergence();

  //! Check if an exception will be thrown on dirty convergence
  bool isExceptionThrownOnDirtyConvergence() const;

  //! Set the convergence tolerance
  void setConvergenceTolerance( const double convergence_tol );

  //! Get the convergence tolerance
  double getConvergenceTolerance() const;

  //! Set the absolute difference tolerance
  void setAbsoluteDifferenceTolerance( const double absolute_diff_tol );

  //! Get the absolute difference tolerance
  double getAbsoluteDifferenceTolerance() const;

  //! Set the distance tolerance
  void setDistanceTolerance( const double distance_tol );

  //! Get the distance tolerance
  double getDistanceTolerance() const;

  //! Generate the primary grid in place
  template<typename STLCompliantContainer, typename Functor>
  void generateInPlace( STLCompliantContainer& primary_grid,
                        const Functor& function ) const;

  //! Generate the primary grid in place (return secondary grids and evaluated function)
  template<typename STLCompliantContainerA,
           typename STLCompliantContainerB,
           typename STLCompliantContainerC,
           typename Functor>
  void generateAndEvaluateInPlace( STLCompliantContainerA& primary_grid,
                                   STLCompliantContainerB& secondary_grids,
                                   STLCompliantContainerC& evaluated_function,
                                   const Functor& function ) const;

  //! Generate the primary grid
  template<typename STLCompliantContainerA,
           typename STLCompliantContainerB,
           typename Functor>
  void generate( STLCompliantContainerA& primary_grid,
                 const STLCompliantContainerB& initial_primary_grid,
                 const Functor& function ) const;

  //! Generate the primary grid (return secondary grids and evaluated function)
  template<typename STLCompliantContainerA,
           typename STLCompliantContainerB,
           typename STLCompliantContainerC,
           typename STLCompliantContainerD,
           typename Functor>
  void generateAndEvaluate( STLCompliantContainerA& primary_grid,
                            STLCompliantContainerB& secondary_grids,
                            STLCompliantContainerC& evaluated_function,
                            const STLCompliantContainerD& initial_primary_grid,
                            const Functor& function ) const;

  //! Generate and evaluate the secondary grid in place
  template<typename STLCompliantContainerA,
           typename STLCompliantContainerB,
           typename Functor>
  void generateAndEvaluateSecondaryInPlace(
                                    STLCompliantContainerA& secondary_grid,
                                    STLCompliantContainerB& evaluated_function,
                                    const double primary_value,
                                    const Functor& function ) const;

protected:

  //! Add critical values to primary grid
  virtual void addCriticalValuesToPrimaryGrid(
                                      std::deque<double>& primary_grid ) const;

  //! Initialize secondary grid at a primary grid point
  virtual void initializeSecondaryGrid(
                                   std::vector<double>& secondary_grid,
                                   const double primary_grid_point ) const = 0;

  // The secondary grid generator
  Utility::GridGenerator<typename TwoDInterpPolicy::ZYInterpPolicy>
  d_secondary_grid_generator;

private:

  // Check for 2D grid convergence
  template<typename STLCompliantContainerA,
           typename STLCompliantContainerB,
           typename Functor>
  bool hasGridConverged(
                    const double primary_value_0,
                    const double primary_value_1,
                    const STLCompliantContainerA& secondary_grid_0,
                    const STLCompliantContainerA& secondary_grid_1,
                    const STLCompliantContainerB& evaluated_function_0,
                    const STLCompliantContainerB& evaluated_function_1,
                    const Functor& function ) const;

  // Check for 2D grid convergence at the intermediate value
  template<typename STLCompliantContainerA,
           typename STLCompliantContainerB>
  bool hasGridConvergedAtSecondaryPoint(
                    const double primary_value_0,
                    const double primary_value_1,
                    const double intermediate_primary_value,
                    const double secondary_value,
                    const double exact_function_value,
                    const STLCompliantContainerA& secondary_grid_0,
                    const STLCompliantContainerA& secondary_grid_1,
                    const STLCompliantContainerB& evaluated_function_0,
                    const STLCompliantContainerB& evaluated_function_1 ) const;

  // Calculate the midpoint between two primary values
  double calculatePrimaryMidpoint( const double primary_value_0,
                                   const double primary_value_1 ) const;

  // Calculate the midpoint between two secondary values
  double calculateSecondaryMidpoint( const double secondary_value_0,
                                     const double secondary_value_1 ) const;

  // Log added primary grid point
  void logAddedPrimaryGridPoint( const double primary_grid_point,
                                 const double primary_grid_point_id ) const;

  // Log secondary grid check
  void logSecondaryGridCheck( const double primary_grid_point_0,
                              const double primary_grid_point_1,
                              const double primary_point,
                              const double secondary_point,
                              const double interp_function_value,
                              const double exact_function_value,
                              const bool converged ) const;

  // The convergence tolerance
  double d_convergence_tol;

  // The absolute difference tolerance
  double d_absolute_diff_tol;

  // The distance tolerance
  double d_distance_tol;

  // Verbose mode
  bool d_verbose_mode_on;

  // Throw exception on dirty convergence
  bool d_throw_exceptions;
};

} // end Utility namespace

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "Utility_TwoDGridGenerator_def.hpp"

//---------------------------------------------------------------------------//

#endif // end UTILITY_TWO_D_GRID_GENERATOR_HPP

//---------------------------------------------------------------------------//
// end Utility_TwoDGridGenerator.hpp
//---------------------------------------------------------------------------//


/**
 * Copyright 2004-present Facebook. All Rights Reserved.
 *
 * This source code is licensed under the BSD-style license found in the
 * LICENSE file in the root directory of this source tree.
 */

#include <gflags/gflags.h>
#include <glog/logging.h>

#include <boost/format.hpp>

#include "source/rig/RigAligner.h"
#include "source/util/Camera.h"
#include "source/util/SystemUtil.h"

using namespace fb360_dep;

const std::string kUsageMessage = R"(
  - Performs a camera-to-camera compare between an input rig and a reference rig.

  - Example:
    ./RigCompare \
    --rig=/path/to/rigs/rig.json \
    --reference=/path/to/rigs/reference.json
)";

DEFINE_string(reference, "", "path to reference rig .json file (required)");
DEFINE_string(rig, "", "path to rig .json file (required)");
DEFINE_bool(skip_align, false, "skip rig alignment before comparing");

void compareRigs(const Camera::Rig& rig, const Camera::Rig& reference) {
  double averagePositionDiff = 0;
  double averageForwardDiff = 0;
  double averageUpDiff = 0;
  double averagePrincipalDiff = 0;
  double averageFocalDiff = 0;
  for (const Camera& camera : rig) {
    const Camera& ref = Camera::findCameraById(camera.id, reference);
    LOG(INFO) << camera.id;

    double currentPositionDiff = (camera.position - ref.position).norm();
    double currentForwardDiff = acos(camera.forward().dot(ref.forward()));
    double currentUpDiff = acos(camera.up().dot(ref.up()));
    double currentPrincipalDiff = (camera.principal - ref.principal).norm();
    double currentFocalDiff = camera.getScalarFocal() - ref.getScalarFocal();

    averagePositionDiff += currentPositionDiff;
    averageForwardDiff += currentForwardDiff;
    averageUpDiff += currentUpDiff;
    averagePrincipalDiff += currentPrincipalDiff;
    averageFocalDiff += currentFocalDiff;

    LOG(INFO) << "- position diff: " << currentPositionDiff;
    LOG(INFO) << "- forward diff (radians): " << currentForwardDiff;
    LOG(INFO) << "- up diff (radians): " <<  currentUpDiff;
    LOG(INFO) << "- principal diff: " <<  currentPrincipalDiff;
    LOG(INFO) << "- focal diff: " << currentFocalDiff;
  }

  LOG(INFO) << "Average:";
  LOG(INFO) << "- position diff: " << averagePositionDiff / rig.size();
  LOG(INFO) << "- forward diff (radians): " <<  averageForwardDiff / rig.size();

  LOG(INFO) << "- up diff (radians): " << averageUpDiff / rig.size();

  LOG(INFO) << "- principal diff: " << averagePrincipalDiff / rig.size();
  LOG(INFO) << "- focal diff: " << averageFocalDiff / rig.size();
}

int main(int argc, char* argv[]) {
  system_util::initDep(argc, argv, kUsageMessage);

  CHECK_NE(FLAGS_rig, "");
  CHECK_NE(FLAGS_reference, "");

  // read the rigs
  Camera::Rig rig = Camera::loadRig(FLAGS_rig);
  Camera::Rig reference = Camera::loadRig(FLAGS_reference);

  if (!FLAGS_skip_align) {
    rig = alignRig(rig, reference);
  }

  compareRigs(rig, reference);

  return EXIT_SUCCESS;
}


// This file is part of the dune-xt-common project:
//   https://github.com/dune-community/dune-xt-common
// Copyright 2009-2018 dune-xt-common developers and contributors. All rights reserved.
// License: Dual licensed as BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)
//      or  GPL-2.0+ (http://opensource.org/licenses/gpl-license)
//          with "runtime exception" (http://www.dune-project.org/license.html)
// Authors:
//   Andreas Buhr    (2014)
//   Felix Schindler (2012 - 2019)
//   René Fritze     (2010 - 2018)
//   Sven Kaulmann   (2013)
//   Tobias Leibner  (2014, 2017 - 2018)

#ifndef DUNE_XT_COMMON_MATH_HH
#define DUNE_XT_COMMON_MATH_HH

#include <vector>
#include <limits>
#include <algorithm>
#include <cstring>
#include <iostream>
#include <type_traits>
#include <complex>

#include <dune/xt/common/disable_warnings.hh>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/format.hpp>
#include <boost/fusion/include/void.hpp>
#include <boost/geometry.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/static_assert.hpp>
#include <dune/xt/common/reenable_warnings.hh>

#include <dune/common/deprecated.hh>
#include <dune/common/promotiontraits.hh>

#include <dune/xt/common/type_traits.hh>

namespace Dune {
namespace XT {
namespace Common {

/**
 * since std::numeric_limits<T>::epsilon() is 0 for integral types
 * use this to get the minimum increment/difference for all basic types
 * (or add specializations as necessary ofc)
 **/
template <class T, bool is_integral = std::is_integral<T>::value>
struct Epsilon
{};

template <class T>
struct Epsilon<T, true>
{
  static const T value;
};

template <class T>
struct Epsilon<T, false>
{
  static const T value;
};

template <>
struct Epsilon<std::string, false>
{
  static const std::string value;
};

template <class T>
const T Epsilon<T, true>::value = T(1);
template <class T>
const T Epsilon<T, false>::value = std::numeric_limits<T>::epsilon();

namespace internal {

/**
 *  Helper struct to compute absolute values of signed and unsigned values,
 *  std::abs is only defined for signed types.
 **/
template <class T, bool isUnsigned = std::is_unsigned<T>::value>
struct AbsoluteValue
{
  static T result(const T& val)
  {
    using std::abs;
    return abs(val);
  }
};

template <class T>
struct AbsoluteValue<T, true>
{
  static T result(const T& val)
  {
    return val;
  }
};

template <class T, bool is_enum = std::is_enum<T>::value>
struct Absretval
{
  typedef T type;
};

template <class T>
struct Absretval<T, true>
{
  typedef typename underlying_type<T>::type type;
};

} // namespace internal

//! drop-in replacement for std::abs, that works for more types
template <class T>
typename internal::Absretval<T>::type abs(const T& val)
{
  typedef typename internal::Absretval<T>::type R;
  return internal::AbsoluteValue<R>::result(static_cast<R>(val));
}

//! very simple, underrun-safe for unsigned types, difference method
template <class T>
T absolute_difference(T a, T b)
{
  return (a > b) ? a - b : b - a;
}

//! a vector wrapper for continiously updating min,max,avg of some element type vector
template <class ElementType>
class MinMaxAvg
{
  static_assert(!is_complex<ElementType>::value, "complex accumulation not supported");

protected:
  typedef MinMaxAvg<ElementType> ThisType;

public:
  MinMaxAvg() {}

  template <class stl_container_type>
  MinMaxAvg(const stl_container_type& elements)
  {
    static_assert((boost::is_same<ElementType, typename stl_container_type::value_type>::value),
                  "cannot assign mismatching types");
    acc_ = std::for_each(elements.begin(), elements.end(), acc_);
  }

  std::size_t count() const
  {
    return boost::accumulators::count(acc_);
  }
  ElementType sum() const
  {
    return boost::accumulators::sum(acc_);
  }
  ElementType min() const
  {
    return boost::accumulators::min(acc_);
  }
  ElementType max() const
  {
    return boost::accumulators::max(acc_);
  }
  ElementType average() const
  {
    // for integer ElementType this just truncates from floating-point
    return ElementType(boost::accumulators::mean(acc_));
  }

  void operator()(const ElementType& el)
  {
    acc_(el);
  }

  void output(std::ostream& stream) const
  {
    stream << boost::format("min: %e\tmax: %e\tavg: %e\n") % min() % max() % average();
  }

protected:
  typedef boost::accumulators::stats<boost::accumulators::tag::max,
                                     boost::accumulators::tag::min,
                                     boost::accumulators::tag::mean,
                                     boost::accumulators::tag::count,
                                     boost::accumulators::tag::sum>
      StatsType;
  boost::accumulators::accumulator_set<ElementType, StatsType> acc_;
};

//! \return var bounded in [min, max]
template <typename T>
constexpr const T& clamp(const T& var, const T& min, const T& max)
{
#if __cplusplus >= 201703L
  return std::clamp(var, min, max);
#else
  return (var < min) ? min : (var > max) ? max : var;
#endif
}

/**
 * \returns: -1 iff val < 0
 *            0 iff val == 0
 *            1 iff val > 0
 */
template <typename T>
int signum(T val)
{
  return (T(0) < val) - (val < T(0));
}

/** enable us to use DXTC::numeric_limits for all types, even when no specialization is avaliable.
 * If there is one, it's used. Otherwise we default to numerical_limtis of double
 **/
template <class T, typename = void>
class numeric_limits : public std::numeric_limits<double>
{};

template <class T>
class numeric_limits<T, typename std::enable_if<std::numeric_limits<T>::is_specialized>::type>
  : public std::numeric_limits<T>
{};

//! forward to std::isnan for general types, overload for complex below
template <class T>
bool isnan(T val)
{
  using std::isnan;
  return isnan(val);
}

//! override isnan for complex here so it doesn't bleed into the std namespace
template <class T>
bool isnan(std::complex<T> val)
{
  return isnan(std::real(val)) || isnan(std::imag(val));
}

//! forward to std::isinf for general types, overload for complex below
template <class T>
bool isinf(T val)
{
  using std::isinf;
  return isinf(val);
}

//! override isinf for complex here so it doesn't bleed into the std namespace
template <class T>
bool isinf(std::complex<T> val)
{
  return isinf(std::real(val)) || isinf(std::imag(val));
}

//! calculates factorial of n
constexpr size_t factorial(size_t n)
{
  return n > 0 ? n * factorial(n - 1) : 1;
}

//! calculates complex conjugate like std::conj, but returns T instead of complex<T>
template <class T>
std::enable_if_t<std::is_arithmetic<T>::value, T> conj(T val)
{
  return val;
}

template <class T>
std::complex<T> conj(std::complex<T> val)
{
  return std::conj(val);
}

//! calculates binomial coefficient for arbitrary n
inline double binomial_coefficient(const double n, const size_t k)
{
  double ret(1);
  for (size_t ii = 1; ii <= k; ++ii)
    ret *= (n + 1 - ii) / ii;
  return ret;
}

template <class T>
T max(const T& left, const T& right)
{
  return std::max(left, right);
}

template <class L, class R>
typename PromotionTraits<L, R>::PromotedType max(const L& left, const R& right)
{
  using T = typename PromotionTraits<L, R>::PromotedType;
  return std::max(T(left), T(right));
}

template <class T>
T min(const T& left, const T& right)
{
  return std::min(left, right);
}

template <class T>
std::enable_if_t<std::is_arithmetic<T>::value, T> minmod(const T left, const T right)
{
  if (std::signbit(left) != std::signbit(right))
    return T(0);
  return std::abs(left) < std::abs(right) ? left : right;
}

template <class T>
std::enable_if_t<std::is_arithmetic<T>::value, T> maxmod(const T left, const T right)
{
  if (std::signbit(left) != std::signbit(right))
    return T(0);
  return std::abs(left) > std::abs(right) ? left : right;
}

template <class L, class R>
typename PromotionTraits<L, R>::PromotedType min(const L& left, const R& right)
{
  using T = typename PromotionTraits<L, R>::PromotedType;
  return std::min(T(left), T(right));
}

// avoid Wfloat-equal warning
template <class FieldType>
inline bool is_zero(const FieldType& val)
{
  return std::equal_to<FieldType>()(val, FieldType(0));
}

} // namespace Common
} // namespace XT
} // namespace Dune

template <class T>
inline std::ostream& operator<<(std::ostream& s, const Dune::XT::Common::MinMaxAvg<T>& d)
{
  d.output(s);
  return s;
}

#endif // DUNE_XT_COMMON_MATH_HH


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SWAPBYTES_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_SWAPBYTES_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/shuffle.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    template<std::size_t N> struct swap_bytes_helper
    {
      template<typename I, typename C>
      struct apply : std::integral_constant < std::size_t, N*(I::value/N)+N-1-I::value%N>
      {};
    };
  }
} }

namespace boost { namespace simd { namespace ext
{

  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF(swapbytes_
                            , (typename A0, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_<bd::integer_<A0>, X>
                            )
  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
    {
      using s0        = typename A0::value_type;
      static const size_t N = A0::static_size*sizeof(s0);
      using i8        = pack<uint8_t, N>;
      using pattern_t = detail::swap_bytes_helper<sizeof(s0)>;

      return bitwise_cast<A0>(shuffle<pattern_t>(bitwise_cast<i8>(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD(swapbytes_
                         , (typename A0, typename X)
                         , bd::cpu_
                         , bs::pack_<bd::ints8_<A0>, X>
                         )
  {
    BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
    {
      return a0;
    }
  };

} } }

#endif


/*! \file Peridigm_EnergyReleaseDamageModel.cpp */

//@HEADER
// ************************************************************************
//
//                             Peridigm
//                 Copyright (2011) Sandia Corporation
//
// Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
// the U.S. Government retains certain rights in this software.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. 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.
//
// 3. Neither the name of the Corporation nor the names of the
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY SANDIA CORPORATION "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 SANDIA CORPORATION OR THE
// 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.
//
// Questions?
// David J. Littlewood   djlittl@sandia.gov
// John A. Mitchell      jamitch@sandia.gov
// Michael L. Parks      mlparks@sandia.gov
// Stewart A. Silling    sasilli@sandia.gov
//
// ************************************************************************
//////////////////////////////////////////////////////////////////////////////////////
// Routine developed for Peridigm by
// DLR Composite Structures and Adaptive Systems
//                                __/|__
//                                /_/_/_/
//            www.dlr.de/fa/en      |/ DLR
//////////////////////////////////////////////////////////////////////////////////////
// Questions?
// Christian Willberg  christian.willberg@dlr.de
// Martin Raedel       martin.raedel@dlr.de
//////////////////////////////////////////////////////////////////////////////////////
//@HEADER

#include "Peridigm_EnergyReleaseDamageModel.hpp"
#include "Peridigm_Field.hpp"
#include "material_utilities.h"
#include <thread>
#include <Teuchos_Assert.hpp>
#include <Epetra_SerialComm.h>
#include <Sacado.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

using namespace std;

PeridigmNS::EnergyReleaseDamageModel::EnergyReleaseDamageModel(const Teuchos::ParameterList& params)
: DamageModel(params),
m_applyThermalStrains(false),
m_modelCoordinatesFieldId(-1),
m_coordinatesFieldId(-1),
m_damageFieldId(-1),
m_bondDamageFieldId(-1),
m_deltaTemperatureFieldId(-1),
m_dilatationFieldId(-1),
m_weightedVolumeFieldId(-1),
m_horizonFieldId(-1),
m_damageModelFieldId(-1),
m_OMEGA(PeridigmNS::InfluenceFunction::self().getInfluenceFunction()) {

    if (params.isParameter("Critical Energy")) {
        m_criticalEnergyTension = params.get<double>("Critical Energy Tension");
        m_criticalEnergyCompression = params.get<double>("Critical Energy Compression");
        m_criticalEnergyShear = params.get<double>("Critical Energy Shear");

    } else {
        if (params.isParameter("Critical Energy Tension"))
            m_criticalEnergyTension = params.get<double>("Critical Energy Tension");
        else
            m_criticalEnergyTension = -1.0;
        if (params.isParameter("Critical Energy Compression"))
            m_criticalEnergyCompression = params.get<double>("Critical Energy Compression");
        else
            m_criticalEnergyCompression = -1.0;
        if (params.isParameter("Critical Energy Shear"))
            m_criticalEnergyShear = params.get<double>("Critical Energy Shear");
        else
            m_criticalEnergyShear = -1.0;
        m_type = 1;
        if (params.isType<string>("Energy Criterion"))
            m_type = 1;
        if (params.isType<string>("Power Law"))
            m_type = 2;
        if (params.isType<string>("Separated"))
            m_type = 3;
    }

    m_pi = 3.14159;

    if (params.isParameter("Thermal Expansion Coefficient")) {
        m_alpha = params.get<double>("Thermal Expansion Coefficient");
        m_applyThermalStrains = true;
    }

    PeridigmNS::FieldManager& fieldManager = PeridigmNS::FieldManager::self();
    m_modelCoordinatesFieldId = fieldManager.getFieldId("Model_Coordinates");
    m_coordinatesFieldId = fieldManager.getFieldId("Coordinates");
    m_volumeFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::CONSTANT, "Volume");
    m_weightedVolumeFieldId = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::CONSTANT, "Weighted_Volume");
    m_dilatationFieldId = fieldManager.getFieldId(PeridigmField::ELEMENT, PeridigmField::SCALAR, PeridigmField::TWO_STEP, "Dilatation");
    m_damageFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::ELEMENT, PeridigmNS::PeridigmField::SCALAR, PeridigmNS::PeridigmField::TWO_STEP, "Damage");
    m_bondDamageFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::BOND, PeridigmNS::PeridigmField::SCALAR, PeridigmNS::PeridigmField::TWO_STEP, "Bond_Damage");
    m_horizonFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::ELEMENT, PeridigmNS::PeridigmField::SCALAR, PeridigmNS::PeridigmField::CONSTANT, "Horizon");
    m_damageModelFieldId = fieldManager.getFieldId(PeridigmNS::PeridigmField::NODE, PeridigmNS::PeridigmField::VECTOR, PeridigmNS::PeridigmField::TWO_STEP, "Damage_Model_Data");
    m_fieldIds.push_back(m_volumeFieldId);
    m_fieldIds.push_back(m_modelCoordinatesFieldId);
    m_fieldIds.push_back(m_coordinatesFieldId);
    m_fieldIds.push_back(m_weightedVolumeFieldId);
    m_fieldIds.push_back(m_dilatationFieldId);
    m_fieldIds.push_back(m_volumeFieldId);
    m_fieldIds.push_back(m_damageFieldId);
    m_fieldIds.push_back(m_damageModelFieldId);
    m_fieldIds.push_back(m_bondDamageFieldId);
    m_fieldIds.push_back(m_horizonFieldId);

}

PeridigmNS::EnergyReleaseDamageModel::~EnergyReleaseDamageModel() {
}

void
PeridigmNS::EnergyReleaseDamageModel::initialize(const double dt,
        const int numOwnedPoints,
        const int* ownedIDs,
        const int* neighborhoodList,
        PeridigmNS::DataManager& dataManager) const {
    double *damage, *bondDamage;

    dataManager.getData(m_damageFieldId, PeridigmField::STEP_NP1)->ExtractView(&damage);
    dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamage);

    // Initialize damage to zero
    int neighborhoodListIndex(0);
    int bondIndex(0);
    int nodeId, numNeighbors;
    int iID, iNID;

    for (iID = 0; iID < numOwnedPoints; ++iID) {
        nodeId = ownedIDs[iID];
        damage[nodeId] = 0.0;
        numNeighbors = neighborhoodList[neighborhoodListIndex++];
        neighborhoodListIndex += numNeighbors;
        for (iNID = 0; iNID < numNeighbors; ++iNID) {
            bondDamage[bondIndex] = 0.0;
            bondIndex += 1;
        }
    }

}

void
PeridigmNS::EnergyReleaseDamageModel::computeDamage(const double dt,
        const int numOwnedPoints,
        const int* ownedIDs,
        const int* neighborhoodList,
        PeridigmNS::DataManager& dataManager) const {

    double *x, *y, *damage, *bondDamageNP1, *horizon;
    double *cellVolume, *weightedVolume, *damageModel;
    double criticalEnergyTension(-1.0), criticalEnergyCompression(-1.0), criticalEnergyShear(-1.0);
    // for temperature dependencies easy to extent
    double *deltaTemperature = NULL;
    double m_alpha = 0;

    dataManager.getData(m_damageFieldId, PeridigmField::STEP_NP1)->ExtractView(&damage);
    dataManager.getData(m_modelCoordinatesFieldId, PeridigmField::STEP_NONE)->ExtractView(&x);
    dataManager.getData(m_coordinatesFieldId, PeridigmField::STEP_NP1)->ExtractView(&y);
    dataManager.getData(m_weightedVolumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&weightedVolume);
    dataManager.getData(m_volumeFieldId, PeridigmField::STEP_NONE)->ExtractView(&cellVolume);
    //////////////////////////////////////////////////////////////
    // transfer of data is done in ComputeDilation --> PeridigmMaterial.cpp
    //////////////////////////////////////////////////////////////
    dataManager.getData(m_damageModelFieldId, PeridigmField::STEP_NP1)->ExtractView(&damageModel);
    dataManager.getData(m_horizonFieldId, PeridigmField::STEP_NONE)->ExtractView(&horizon);
    ////////////////////////////////////
    ////////////////////////////////////
    // transfer of data is done in ComputeDilation --> PeridigmMaterial.cpp
    dataManager.getData(m_bondDamageFieldId, PeridigmField::STEP_NP1)->ExtractView(&bondDamageNP1);

    ////////////////////////////////////////////////////
    double trialDamage(0.0);
    int neighborhoodListIndex(0), bondIndex(0);
    int nodeId, numNeighbors, neighborID, iID, iNID;
    double totalDamage;
    double alphaP1, alphaP2;
    double nodeInitialX[3], nodeCurrentX[3], relativeExtension(0.0);
    double bondEnergyIsotropic(0.0), bondEnergyDeviatoric(0.0);
    double omegaP1, omegaP2;
    double critDev, critIso, critComp;
    double BulkMod1, BulkMod2;
    double degradationFactor = 1.0; // Optional parameter if bond should be degradated and not fully destroyed instantaneously
    double avgHorizon, quadhorizon;
    //---------------------------
    // INITIALIZE PROCESS STEP t
    //---------------------------

    if (m_criticalEnergyTension > 0.0)
        criticalEnergyTension = m_criticalEnergyTension;
    if (m_criticalEnergyCompression > 0.0)
        criticalEnergyCompression = m_criticalEnergyCompression;
    if (m_criticalEnergyShear > 0.0)
        criticalEnergyShear = m_criticalEnergyShear;

    // Update the bond damage
    // Break bonds if the bond energy potential is greater than the critical bond energy potential
    //---------------------------
    // DAMAGE ANALYSIS
    //---------------------------
    bondIndex = 0;

    for (iID = 0; iID < numOwnedPoints; ++iID) {
        numNeighbors = neighborhoodList[neighborhoodListIndex++];

        nodeId = ownedIDs[iID];
        nodeInitialX[0] = x[nodeId*3];
        nodeInitialX[1] = x[nodeId*3+1];
        nodeInitialX[2] = x[nodeId*3+2];
        nodeCurrentX[0] = y[nodeId*3];
        nodeCurrentX[1] = y[nodeId*3+1];
        nodeCurrentX[2] = y[nodeId*3+2];

        double dilatationP1 = damageModel[3*nodeId];

        alphaP1 = 15.0 * damageModel[3*nodeId+2]; // weightedVolume is already included in --> Perdigm_Material.cpp;

        for (iNID = 0; iNID < numNeighbors; ++iNID) {

            trialDamage = 0.0;
            neighborID = neighborhoodList[neighborhoodListIndex++];

            double zeta =
            distance(nodeInitialX[0], nodeInitialX[1], nodeInitialX[2],
                 x[neighborID*3], x[neighborID*3+1], x[neighborID*3+2]);

            double dY =
            distance(nodeCurrentX[0], nodeCurrentX[1], nodeCurrentX[2],
                 y[neighborID*3], y[neighborID*3+1], y[neighborID*3+2]);

            relativeExtension = (dY - zeta)/zeta;
             // the direction switches between both bonds. This results in a switch of the forces
             // as well. Therefore, all forces and bond deformations are normalized.
            double eP = dY - zeta;
            //double normZeta = sqrt(zeta*zeta);

            alphaP2 = 15.0 * damageModel[3*neighborID+2]; // weightedVolume is already included in --> Perdigm_Material.cpp;

            BulkMod1 = damageModel[3*nodeId+1];     // weightedVolume is already included in the variable --> Perdigm_Material.cpp;
            BulkMod2 = damageModel[3*neighborID+1]; // weightedVolume is already included in the variable --> Perdigm_Material.cpp;

            double dilatationP2 = damageModel[3*neighborID];

            omegaP1 = MATERIAL_EVALUATION::scalarInfluenceFunction(zeta, horizon[nodeId]);
            omegaP2 = MATERIAL_EVALUATION::scalarInfluenceFunction(zeta, horizon[neighborID]);

            double eiP1 = dilatationP1 * zeta / 3.0;
            double eiP2 = dilatationP2 * zeta / 3.0;
            double tiP1 = 3*BulkMod1*omegaP1*dilatationP1*zeta;
            double tiP2 = 3*BulkMod2*omegaP2*dilatationP2*zeta;

            double edP1 = eP - sqrt(eiP1*eiP1);
            double edP2 = eP - sqrt(eiP2*eiP2);
            double tdP1 = omegaP1*alphaP1*edP1;
            double tdP2 = omegaP2*alphaP2*edP2;
            // absolute values of energy, because it is positive and coordinate errors are avoided.
            bondEnergyIsotropic  = (1.0 - bondDamageNP1[bondIndex])*(sqrt(tiP1*eiP1*tiP1*eiP1) + sqrt(tiP2*eiP2*tiP2*eiP2));
            bondEnergyDeviatoric = (1.0 - bondDamageNP1[bondIndex])*(sqrt(tdP1*edP1*tdP2*edP2) + sqrt(tdP2*edP2*tdP2*edP2));
            // the average horizon is taken, if multiple horizons are used
            avgHorizon = 0.5*(horizon[nodeId]+horizon[neighborID]);

            // geometrical part of the energy value
            quadhorizon =  16.0 /( m_pi * avgHorizon * avgHorizon * avgHorizon * avgHorizon );
            // the factor 16 can be split in three parts:
            // 4 comes from the integration from Foster et al. (2009) Journal for Multiscale Computational Engineering;
            // 2 comes from the force split motivated by the bond based formulation Bobaru et al. (2017) "Handbook of Peridynamik Modeling", page 48 ;
            // 2 comes from the energy formulation itself
            critIso = 0.0;
            if (relativeExtension>0&&criticalEnergyTension != -1.0){
               critIso = (bondEnergyIsotropic/(criticalEnergyTension*quadhorizon));
            }
            critDev = 0.0;
            if (criticalEnergyShear != -1.0){
               critDev = (bondEnergyDeviatoric/(criticalEnergyShear*quadhorizon));
            }
            critComp = 0.0;
            if (relativeExtension < 0.0 && criticalEnergyCompression != -1.0){
                critComp = (bondEnergyIsotropic/(criticalEnergyCompression*quadhorizon));
                critIso = 0.0;
            }
            if (m_type == 1){ // Energy Criterion by Foster et al.(2009) Journal for Multiscale Computational Engineering;
                double bondEnergy = sqrt((tdP1+tiP1)*(tdP1+tiP1)*eP*eP) + sqrt((tdP2 + tiP2)*(tdP2 + tiP2)*eP*eP);
                critIso = bondEnergy/(criticalEnergyTension*quadhorizon);
                if (m_criticalEnergyTension > 0.0 && critIso > 1.0 ) {
                    trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
            }

            if (m_type == 2){// Power Law

                if (m_criticalEnergyTension > 0.0 &&critIso*critIso + critDev*critDev + critComp*critComp > 1.0) {
                   trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
            }
            if (m_type == 3){ // Separated
                if (m_criticalEnergyTension > 0.0  &&critIso > 1.0) {
                   trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
                if (m_criticalEnergyTension > 0.0 && critDev > 1.0) {
                   trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
                if (m_criticalEnergyCompression > 0.0  && critComp > 1.0) {
                   trialDamage = bondDamageNP1[bondIndex] + degradationFactor;
                }
            }

            if (trialDamage > bondDamageNP1[bondIndex]) {
                if (trialDamage>1)trialDamage = 1;
                bondDamageNP1[bondIndex] = trialDamage;

            }

            bondIndex += 1;

            }

        }

    //  Update the element damage (percent of bonds broken)

    neighborhoodListIndex = 0;
    bondIndex = 0;
    for (iID = 0; iID < numOwnedPoints; ++iID) {
        nodeId = ownedIDs[iID];
        numNeighbors = neighborhoodList[neighborhoodListIndex++];
        //neighborhoodListIndex += numNeighbors;
        damageModel[3*nodeId] = 0.0;
        damageModel[3*nodeId+1] = 0.0;
        damageModel[3*nodeId+2] = 0.0;
        totalDamage = 0.0;
        for (iNID = 0; iNID < numNeighbors; ++iNID) {

            neighborID = neighborhoodList[neighborhoodListIndex++];
            // must be zero to avoid synchronization errors
            damageModel[3*neighborID] = 0.0;
            damageModel[3*neighborID+1] = 0.0;
            damageModel[3*neighborID+2] = 0.0;

            totalDamage += bondDamageNP1[bondIndex];

            bondIndex += 1;
        }
        if (numNeighbors > 0)
            totalDamage /= numNeighbors;
        else
            totalDamage = 0.0;
        damage[nodeId] = totalDamage;
    }
}