// Copyright (c) 2008, Willow Garage, Inc.
//
// 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/89069bc/sensor_msgs/include/sensor_msgs/fill_image.h

#ifndef SENSOR_MSGS__FILL_IMAGE_HPP_
#define SENSOR_MSGS__FILL_IMAGE_HPP_

#include <cstring>
#include <string>

#include "sensor_msgs/msg/image.hpp"
#include "sensor_msgs/image_encodings.hpp"

namespace sensor_msgs
{
/// Fill an image message.
/**
 * \param[out] image Image to be filled.
 * \param[in] encoding_arg Encoding type, such as sensor_msgs::image_encodings::RGB8.
 * \param[in] rows_arg Number of rows.
 * \param[in] cols_arg Number of columns.
 * \param[in] step_arg Step size.
 * \param[in] data_arg Data to fill image with.
 * \return True if successful.
 */
static inline bool fillImage(
  msg::Image & image,
  const std::string & encoding_arg,
  uint32_t rows_arg,
  uint32_t cols_arg,
  uint32_t step_arg,
  const void * data_arg)
{
  image.encoding = encoding_arg;
  image.height = rows_arg;
  image.width = cols_arg;
  image.step = step_arg;
  size_t st0 = (step_arg * rows_arg);
  image.data.resize(st0);
  std::memcpy(&image.data[0], data_arg, st0);

  image.is_bigendian = 0;
  return true;
}

/// Clear the data of an image message.
/**
 * \details All fields but `data` are kept the same.
 * \param[out] image Image to be cleared.
 */
static inline void clearImage(msg::Image & image)
{
  image.data.resize(0);
}
}  // namespace sensor_msgs

#endif  // SENSOR_MSGS__FILL_IMAGE_HPP_
