// Copyright (c) 2013, Open Source Robotics Foundation
//
// 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 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.

// this file is originally ported from ROS1:
// https://github.com/ros/common_msgs/blob/4148d041bae4f284819b980ff50577344147cd5b/sensor_msgs/test/main.cpp

#include <gtest/gtest.h>

#include <vector>

#include "sensor_msgs/msg/point_cloud2.hpp"
#include "sensor_msgs/point_cloud2_iterator.hpp"

TEST(sensor_msgs, PointCloud2Iterator)
{
  // Create a dummy PointCloud2
  size_t n_points = 4;
  sensor_msgs::msg::PointCloud2 cloud_msg_1, cloud_msg_2;
  cloud_msg_1.height = static_cast<uint32_t>(n_points);
  cloud_msg_1.width = 1;
  sensor_msgs::PointCloud2Modifier modifier(cloud_msg_1);
  modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
  cloud_msg_2 = cloud_msg_1;

  // Fill 1 by hand
  float point_data_raw[] = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0};
  std::vector<float> point_data(point_data_raw, point_data_raw + 3 * n_points);
  // colors in RGB order
  uint8_t color_data_raw[] = {40, 80, 120, 160, 200, 240, 20, 40, 60, 80, 100, 120};
  std::vector<uint8_t> color_data(color_data_raw, color_data_raw + 3 * n_points);

  float * data = reinterpret_cast<float *>(&cloud_msg_1.data.front());
  for (size_t n = 0, i = 0; n < n_points; ++n) {
    for (; i < 3 * (n + 1); ++i) {
      *(data++) = point_data[i];
    }
    // Add an extra float of padding
    ++data;
    uint8_t * bgr = reinterpret_cast<uint8_t *>(data++);
    // add the colors in order BGRA like PCL
    size_t j_max = 2;
    for (size_t j = 0; j <= j_max; ++j) {
      *(bgr++) = color_data[3 * n + (j_max - j)];
    }
    // Add 3 extra floats of padding
    data += 3;
  }

  // Fill 2 using an iterator
  sensor_msgs::PointCloud2Iterator<float> iter_x(cloud_msg_2, "x");
  sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(cloud_msg_2, "r");
  sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(cloud_msg_2, "g");
  sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(cloud_msg_2, "b");
  for (size_t i = 0; i < n_points; ++i, ++iter_x, ++iter_r, ++iter_g, ++iter_b) {
    for (size_t j = 0; j < 3; ++j) {
      iter_x[j] = point_data[j + 3 * i];
    }
    *iter_r = color_data[3 * i];
    *iter_g = color_data[3 * i + 1];
    *iter_b = color_data[3 * i + 2];
  }


  // Check the values using an iterator
  sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_x(cloud_msg_1, "x"), iter_const_2_x(
    cloud_msg_2, "x");
  sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_y(cloud_msg_1, "y"), iter_const_2_y(
    cloud_msg_2, "y");
  sensor_msgs::PointCloud2ConstIterator<float> iter_const_1_z(cloud_msg_1, "z"), iter_const_2_z(
    cloud_msg_2, "z");
  sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_r(cloud_msg_1, "r"), iter_const_2_r(
    cloud_msg_2, "r");
  sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_g(cloud_msg_1, "g"), iter_const_2_g(
    cloud_msg_2, "g");
  sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_const_1_b(cloud_msg_1, "b"), iter_const_2_b(
    cloud_msg_2, "b");

  size_t i = 0;
  for (; iter_const_1_x != iter_const_1_x.end();
    ++i, ++iter_const_1_x, ++iter_const_2_x, ++iter_const_1_y, ++iter_const_2_y,
    ++iter_const_1_z, ++iter_const_2_z, ++iter_const_1_r, ++iter_const_1_g, ++iter_const_1_b)
  {
    EXPECT_EQ(*iter_const_1_x, *iter_const_2_x);
    EXPECT_EQ(*iter_const_1_x, point_data[0 + 3 * i]);
    EXPECT_EQ(*iter_const_1_y, *iter_const_2_y);
    EXPECT_EQ(*iter_const_1_y, point_data[1 + 3 * i]);
    EXPECT_EQ(*iter_const_1_z, *iter_const_2_z);
    EXPECT_EQ(*iter_const_1_z, point_data[2 + 3 * i]);
    EXPECT_EQ(*iter_const_1_r, *iter_const_2_r);
    EXPECT_EQ(*iter_const_1_r, color_data[3 * i + 0]);
    EXPECT_EQ(*iter_const_1_g, *iter_const_2_g);
    EXPECT_EQ(*iter_const_1_g, color_data[3 * i + 1]);
    EXPECT_EQ(*iter_const_1_b, *iter_const_2_b);
    EXPECT_EQ(*iter_const_1_b, color_data[3 * i + 2]);
    // This is to test the different operators
    ++iter_const_2_r;
    iter_const_2_g += 1;
    iter_const_2_b = iter_const_2_b + 1;
  }
  EXPECT_EQ(i, n_points);
}

TEST(sensor_msgs, PointCloud2Resize)
{
  // Check if size will retain when width and height given explicitly (width == 1)
  size_t n_points = 4;
  sensor_msgs::msg::PointCloud2 cloud_msg_1;
  cloud_msg_1.height = static_cast<uint32_t>(n_points);
  cloud_msg_1.width = 1;
  sensor_msgs::PointCloud2Modifier modifier(cloud_msg_1);
  modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
  modifier.resize(1 * n_points);
  EXPECT_EQ(static_cast<uint32_t>(1), cloud_msg_1.width);
  EXPECT_EQ(static_cast<uint32_t>(4), cloud_msg_1.height);
  modifier.resize(static_cast<uint32_t>(1), static_cast<uint32_t>(n_points));
  EXPECT_EQ(static_cast<uint32_t>(1), cloud_msg_1.width);
  EXPECT_EQ(static_cast<uint32_t>(4), cloud_msg_1.height);

  // Check if size will retain when width and height given explicitly (height == 1)
  size_t n_points2 = 5;
  sensor_msgs::msg::PointCloud2 cloud_msg_2;
  cloud_msg_2.height = 1;
  cloud_msg_2.width = static_cast<uint32_t>(n_points2);
  sensor_msgs::PointCloud2Modifier modifier2(cloud_msg_2);
  modifier2.setPointCloud2FieldsByString(2, "xyz", "rgb");
  modifier2.resize(1 * n_points2);
  EXPECT_EQ(static_cast<uint32_t>(5), cloud_msg_2.width);
  EXPECT_EQ(static_cast<uint32_t>(1), cloud_msg_2.height);
  EXPECT_EQ(static_cast<uint32_t>(160), cloud_msg_2.row_step);
  modifier2.resize(static_cast<uint32_t>(n_points2), static_cast<uint32_t>(1));
  EXPECT_EQ(static_cast<uint32_t>(5), cloud_msg_2.width);
  EXPECT_EQ(static_cast<uint32_t>(1), cloud_msg_2.height);
  EXPECT_EQ(static_cast<uint32_t>(160), cloud_msg_2.row_step);

  // Check if resize works when width and height are changed
  sensor_msgs::msg::PointCloud2 cloud_msg_3;
  cloud_msg_3.height = 3;
  cloud_msg_3.width = 3;
  sensor_msgs::PointCloud2Modifier modifier3(cloud_msg_3);
  modifier3.setPointCloud2FieldsByString(2, "xyz", "rgb");
  modifier3.resize(10);
  EXPECT_EQ(static_cast<uint32_t>(10), cloud_msg_3.width);
  EXPECT_EQ(static_cast<uint32_t>(1), cloud_msg_3.height);
  EXPECT_EQ(static_cast<uint32_t>(320), cloud_msg_3.row_step);
  modifier3.resize(static_cast<uint32_t>(11), static_cast<uint32_t>(11));
  EXPECT_EQ(static_cast<uint32_t>(11), cloud_msg_3.width);
  EXPECT_EQ(static_cast<uint32_t>(11), cloud_msg_3.height);
  EXPECT_EQ(static_cast<uint32_t>(3872), cloud_msg_3.row_step);
}