/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2010-2012, Willow Garage, Inc.
 *
 *  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 copyright holder(s) nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id$
 */

#pragma once

#include <Eigen/Core>

/** \file pcl_tests.h
  * Helper macros for testing equality of various data fields in PCL points */

namespace pcl
{

  /** test_macros.h provide helper macros for testing vectors, matrices etc.
    * We took some liberty with upcasing names to make them look like googletest
    * macros names so that reader is not confused.
    *
    * This file also provides a family of googletest-style macros for asserting
    * equality or nearness of xyz, normal, and rgba fields.
    *
    * \author Nizar Sallem, Sergey Alexandrov
    */

  namespace test
  {

    template <typename V1, typename V2>
    void EXPECT_EQ_VECTORS (const V1& v1, const V2& v2)
    {
      SCOPED_TRACE("EXPECT_EQ_VECTORS failed");
      EXPECT_EQ (v1.size (), v2.size ());
      std::size_t length = v1.size ();
      for (std::size_t i = 0; i < length; ++i)
        EXPECT_EQ (v1[i], v2[i]);
    }

    template <typename V1, typename V2, typename Scalar>
    void EXPECT_NEAR_VECTORS (const V1& v1, const V2& v2, const Scalar& epsilon)
    {
      SCOPED_TRACE("EXPECT_NEAR_VECTORS failed");
      EXPECT_EQ (v1.size (), v2.size ());
      std::size_t length = v1.size ();
      for (std::size_t i = 0; i < length; ++i)
        EXPECT_NEAR (v1[i], v2[i], epsilon);
    }

    namespace internal
    {

      template <typename Point1T, typename Point2T>
      ::testing::AssertionResult XYZEQ (const char* expr1,
                                        const char* expr2,
                                        const Point1T& p1,
                                        const Point2T& p2)
      {
        if ((p1).getVector3fMap ().cwiseEqual ((p2).getVector3fMap ()).all ())
          return ::testing::AssertionSuccess ();
        return ::testing::AssertionFailure ()
               << "Value of: " << expr2 << ".getVector3fMap ()" << std::endl
               << "  Actual: " << p2.getVector3fMap ().transpose () << std::endl
               << "Expected: " << expr1 << ".getVector3fMap ()" << std::endl
               << "Which is: " << p1.getVector3fMap ().transpose ();
      }

      template <typename Point1T, typename Point2T>
      ::testing::AssertionResult XYZNear (const char* expr1,
                                          const char* expr2,
                                          const char* abs_error_expr,
                                          const Point1T& p1,
                                          const Point2T& p2,
                                          double abs_error)
      {
        const Eigen::Vector3f diff = ((p1).getVector3fMap () -
                                      (p2).getVector3fMap ()).cwiseAbs ();
        if ((diff.array () < abs_error).all ())
          return ::testing::AssertionSuccess ();
        return ::testing::AssertionFailure ()
               << "Some of the element-wise differences exceed " << abs_error_expr
               << " (which evaluates to " << abs_error << ")" << std::endl
               << "Difference: " << diff.transpose () << std::endl
               << "  Value of: " << expr2 << ".getVector3fMap ()" << std::endl
               << "    Actual: " << p2.getVector3fMap ().transpose () << std::endl
               << "  Expected: " << expr1 << ".getVector3fMap ()" << std::endl
               << "  Which is: " << p1.getVector3fMap ().transpose ();
      }

      template <typename Point1T, typename Point2T>
      ::testing::AssertionResult NormalEQ (const char* expr1,
                                           const char* expr2,
                                           const Point1T& p1,
                                           const Point2T& p2)
      {
        if ((p1).getNormalVector3fMap ().cwiseEqual ((p2).getNormalVector3fMap ()).all ())
          return ::testing::AssertionSuccess ();
        return ::testing::AssertionFailure ()
               << "Value of: " << expr2 << ".getNormalVector3fMap ()" << std::endl
               << "  Actual: " << p2.getNormalVector3fMap ().transpose () << std::endl
               << "Expected: " << expr1 << ".getNormalVector3fMap ()" << std::endl
               << "Which is: " << p1.getNormalVector3fMap ().transpose ();
      }

      template <typename Point1T, typename Point2T>
      ::testing::AssertionResult NormalNear (const char* expr1,
                                             const char* expr2,
                                             const char* abs_error_expr,
                                             const Point1T& p1,
                                             const Point2T& p2,
                                             double abs_error)
      {
        const Eigen::Vector3f diff = ((p1).getNormalVector3fMap () -
                                      (p2).getNormalVector3fMap ()).cwiseAbs ();
        if ((diff.array () < abs_error).all ())
          return ::testing::AssertionSuccess ();
        return ::testing::AssertionFailure ()
               << "Some of the element-wise differences exceed " << abs_error_expr
               << " (which evaluates to " << abs_error << ")" << std::endl
               << "Difference: " << diff.transpose () << std::endl
               << "  Value of: " << expr2 << ".getNormalVector3fMap ()" << std::endl
               << "    Actual: " << p2.getNormalVector3fMap ().transpose () << std::endl
               << "  Expected: " << expr1 << ".getNormalVector3fMap ()" << std::endl
               << "  Which is: " << p1.getNormalVector3fMap ().transpose ();
      }

      template <typename Point1T, typename Point2T>
      ::testing::AssertionResult RGBEQ (const char* expr1,
                                         const char* expr2,
                                         const Point1T& p1,
                                         const Point2T& p2)
      {
        if ((p1).getRGBVector3i ().cwiseEqual ((p2).getRGBVector3i ()).all ())
          return ::testing::AssertionSuccess ();
        return ::testing::AssertionFailure ()
               << "Value of: " << expr2 << ".getRGBVector3i ()" << std::endl
               << "  Actual: " << p2.getRGBVector3i ().transpose () << std::endl
               << "Expected: " << expr1 << ".getRGBVector3i ()" << std::endl
               << "Which is: " << p1.getRGBVector3i ().transpose ();
      }

      template <typename Point1T, typename Point2T>
      ::testing::AssertionResult RGBAEQ (const char* expr1,
                                         const char* expr2,
                                         const Point1T& p1,
                                         const Point2T& p2)
      {
        if ((p1).getRGBAVector4i ().cwiseEqual ((p2).getRGBAVector4i ()).all ())
          return ::testing::AssertionSuccess ();
        return ::testing::AssertionFailure ()
               << "Value of: " << expr2 << ".getRGBAVector4i ()" << std::endl
               << "  Actual: " << p2.getRGBAVector4i ().transpose () << std::endl
               << "Expected: " << expr1 << ".getRGBAVector4i ()" << std::endl
               << "Which is: " << p1.getRGBAVector4i ().transpose ();
      }

      template <typename PointCloud1T, typename PointCloud2T>
      ::testing::AssertionResult MetaDataEQ (const char* expr1,
                                             const char* expr2,
                                             const PointCloud1T& p1,
                                             const PointCloud2T& p2)
      {
        if (!(p1.header == p2.header))
          return ::testing::AssertionFailure () << "Headers are different";
        if (p1.width != p2.width)
          return ::testing::AssertionFailure ()
                 << "Value of: " << expr2 << ".width" << std::endl
                 << "  Actual: " << p2.width << std::endl
                 << "Expected: " << expr1 << ".width" << std::endl
                 << "Which is: " << p1.width << std::endl;
        if (p1.height != p2.height)
          return ::testing::AssertionFailure ()
                 << "Value of: " << expr2 << ".height" << std::endl
                 << "  Actual: " << p2.height << std::endl
                 << "Expected: " << expr1 << ".height" << std::endl
                 << "Which is: " << p1.height << std::endl;
        if (p1.is_dense != p2.is_dense)
          return ::testing::AssertionFailure ()
                 << "Value of: " << expr2 << ".is_dense" << std::endl
                 << "  Actual: " << p2.is_dense << std::endl
                 << "Expected: " << expr1 << ".is_dense" << std::endl
                 << "Which is: " << p1.is_dense << std::endl;
        if (p1.sensor_origin_ != p2.sensor_origin_)
          return ::testing::AssertionFailure () << "Sensor origins are different";
        if (p1.sensor_orientation_.coeffs () != p2.sensor_orientation_.coeffs ())
          return ::testing::AssertionFailure () << "Sensor orientations are different";
        return ::testing::AssertionSuccess ();
      }


        template<typename V>
        ::testing::AssertionResult VectorContainsAll(const char*  expr1, const char* expr2, const std::vector<V>& elements, const std::vector<V>& v) {
            for(const V& item : elements) {
                if(std::find(v.cbegin(), v.cend(), item)==v.cend()) {

                    std::ostringstream vec_rep;
                    std::copy(v.cbegin(), v.cend()-1, std::ostream_iterator<V>(vec_rep, ", "));
                    vec_rep<<v.back();

                    std::ostringstream elements_rep;
                    std::copy(elements.cbegin(), elements.cend()-1, std::ostream_iterator<V>(elements_rep, ", "));
                    elements_rep << elements.back();

                    return ::testing::AssertionFailure ()
                            << "Actual       : " << expr2 << std::endl
                            << "contains     : " << vec_rep.str() << std::endl
                            << "Target set   : " << expr1 << std::endl
                            << "contains     : " << elements_rep.str() << std::endl;
                }
            }
            return ::testing::AssertionSuccess ();
        }

        template<typename V>
        ::testing::AssertionResult VectorDoesNotContain(const char*  expr1, const char*  expr2, const std::vector<V>& elements, const std::vector<V>& v) {
            for(const V& item : elements) {
                if(std::find(v.cbegin(), v.cend(), item)!=v.cend()) {
                    std::ostringstream vec_rep;
                    std::copy(v.cbegin(), v.cend()-1, std::ostream_iterator<V>(vec_rep, ", "));
                    vec_rep<<v.back();

                    std::ostringstream elements_rep;
                    std::copy(elements.cbegin(), elements.cend()-1, std::ostream_iterator<V>(elements_rep, ", "));
                    elements_rep << elements.back();

                    return ::testing::AssertionFailure ()
                            << "Actual       : " << expr2 << std::endl
                            << "contains     : " << vec_rep.str() << std::endl
                            << "Forbidden set: " << expr1 << std::endl
                            << "contains     : " << elements_rep.str() << std::endl;
                }
            }
            return ::testing::AssertionSuccess ();
        }

    }

  }

}

/// Expect that each of x, y, and z fields are equal in
/// two points.
#define EXPECT_XYZ_EQ(expected, actual)                  \
  EXPECT_PRED_FORMAT2(::pcl::test::internal::XYZEQ,      \
                      (expected), (actual))

/// Assert that each of x, y, and z fields are equal in
/// two points.
#define ASSERT_XYZ_EQ(expected, actual)                  \
  ASSERT_PRED_FORMAT2(::pcl::test::internal::XYZEQ,      \
                      (expected), (actual))

/// Expect that differences between x, y, and z fields in
/// two points are each within abs_error.
#define EXPECT_XYZ_NEAR(expected, actual, abs_error)     \
  EXPECT_PRED_FORMAT3(::pcl::test::internal::XYZNear,    \
                      (expected), (actual), abs_error)

/// Assert that differences between x, y, and z fields in
/// two points are each within abs_error.
#define ASSERT_XYZ_NEAR(expected, actual, abs_error)     \
  ASSERT_PRED_FORMAT3(::pcl::test::internal::XYZNear,    \
                      (expected), (actual), abs_error)

/// Expect that each of normal_x, normal_y, and normal_z
/// fields are equal in two points.
#define EXPECT_NORMAL_EQ(expected, actual)               \
  EXPECT_PRED_FORMAT2(::pcl::test::internal::NormalEQ,   \
                      (expected), (actual))

/// Assert that each of normal_x, normal_y, and normal_z
/// fields are equal in two points.
#define ASSERT_NORMAL_EQ(expected, actual)               \
  ASSERT_PRED_FORMAT2(::pcl::test::internal::NormalEQ,   \
                      (expected), (actual))

/// Expect that differences between normal_x, normal_y,
/// and normal_z fields in two points are each within
/// abs_error.
#define EXPECT_NORMAL_NEAR(expected, actual, abs_error)  \
  EXPECT_PRED_FORMAT3(::pcl::test::internal::NormalNear, \
                      (expected), (actual), abs_error)

/// Assert that differences between normal_x, normal_y,
/// and normal_z fields in two points are each within
/// abs_error.
#define ASSERT_NORMAL_NEAR(expected, actual, abs_error)  \
  ASSERT_PRED_FORMAT3(::pcl::test::internal::NormalNear, \
                      (expected), (actual), abs_error)

/// Expect that each of r, g, and b fields are equal in
/// two points.
#define EXPECT_RGB_EQ(expected, actual)                  \
  EXPECT_PRED_FORMAT2(::pcl::test::internal::RGBEQ,      \
                      (expected), (actual))

/// Assert that each of r, g, and b fields are equal in
/// two points.
#define ASSERT_RGB_EQ(expected, actual)                  \
  ASSERT_PRED_FORMAT2(::pcl::test::internal::RGBEQ,      \
                      (expected), (actual))

/// Expect that each of r, g, b, and a fields are equal
/// in two points.
#define EXPECT_RGBA_EQ(expected, actual)                 \
  EXPECT_PRED_FORMAT2(::pcl::test::internal::RGBAEQ,     \
                      (expected), (actual))

/// Assert that each of r, g, b, and a fields are equal
/// in two points.
#define ASSERT_RGBA_EQ(expected, actual)                 \
  ASSERT_PRED_FORMAT2(::pcl::test::internal::RGBAEQ,     \
                      (expected), (actual))

/// Assert that the metadata (header, width, height,
/// is_dense, sensor origin and orientation) are equal
/// in two point clouds.
#define ASSERT_METADATA_EQ(expected, actual)             \
  ASSERT_PRED_FORMAT2(::pcl::test::internal::MetaDataEQ, \
                      expected, actual)

/// Expect that the metadata (header, width, height,
/// is_dense, sensor origin and orientation) are equal
/// in two point clouds.
#define EXPECT_METADATA_EQ(expected, actual)             \
  EXPECT_PRED_FORMAT2(::pcl::test::internal::MetaDataEQ, \
                      expected, actual)


/// Expect that the vector contains all elements
/// from the expected vector.
#define EXPECT_VECTOR_CONTAINS_ALL(expected, actual)            \
    EXPECT_PRED_FORMAT2(::pcl::test::internal::VectorContainsAll, \
                        expected, actual)


/// Expect that the vector does not contain any element
/// from the expected vector.
#define EXPECT_VECTOR_DOES_NOT_CONTAIN(expected, actual)           \
    EXPECT_PRED_FORMAT2(::pcl::test::internal::VectorDoesNotContain, \
                        expected, actual)



/// Assert that the vector contains all elements
/// from the expected vector.
#define ASSERT_VECTOR_CONTAINS_ALL(expected, actual)            \
    ASSERT_PRED_FORMAT2(::pcl::test::internal::VectorContainsAll, \
                        expected, actual)


/// Assert that the vector does not contain any element
/// from the expected vector.
#define ASSERT_VECTOR_DOES_NOT_CONTAIN(expected, actual)           \
    ASSERT_PRED_FORMAT2(::pcl::test::internal::VectorDoesNotContain, \
                        expected, actual)