/*
 * Software License Agreement (BSD License)
 *
 *  Point Cloud Library (PCL) - www.pointclouds.org
 *  Copyright (c) 2012-, Open Perception, 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.
 *
 */

#ifndef PCL_LZF_IMAGE_IO_HPP_
#define PCL_LZF_IMAGE_IO_HPP_

#include <pcl/console/print.h>
#include <pcl/common/utils.h> // pcl::utils::ignore
#include <pcl/io/debayer.h>

#include <cstddef>
#include <cstdint>
#include <limits>
#include <string>
#include <vector>

#define CLIP_CHAR(c) static_cast<unsigned char> ((c)>255?255:(c)<0?0:(c))


namespace pcl
{

namespace io
{

template <typename PointT> bool
LZFDepth16ImageReader::read (
    const std::string &filename, pcl::PointCloud<PointT> &cloud)
{
  std::uint32_t uncompressed_size;
  std::vector<char> compressed_data;
  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
  {
    PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
    return (false);
  }

  if (uncompressed_size != getWidth () * getHeight () * 2)
  {
    PCL_DEBUG ("[pcl::io::LZFDepth16ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFDepth16ImageReader::read] Are you sure %s is a 16-bit depth PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight () * 2, filename.c_str (), getImageType ().c_str ());
    return (false);
  }

  std::vector<char> uncompressed_data (uncompressed_size);
  decompress (compressed_data, uncompressed_data);

  if (uncompressed_data.empty ())
  {
    PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
    return (false);
  }

  // Copy to PointT
  cloud.width    = getWidth ();
  cloud.height   = getHeight ();
  cloud.is_dense = true;
  cloud.resize (getWidth () * getHeight ());
  int depth_idx = 0, point_idx = 0;
  double constant_x = 1.0 / parameters_.focal_length_x,
         constant_y = 1.0 / parameters_.focal_length_y;
  for (std::uint32_t v = 0; v < cloud.height; ++v)
  {
    for (std::uint32_t u = 0; u < cloud.width; ++u, ++point_idx, depth_idx += 2)
    {
      PointT &pt = cloud[point_idx];
      unsigned short val;
      memcpy (&val, &uncompressed_data[depth_idx], sizeof (unsigned short));
      if (val == 0)
      {
        pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
        cloud.is_dense = false;
        continue;
      }

      pt.z = static_cast<float> (val * z_multiplication_factor_);
      pt.x = (static_cast<float> (u) - static_cast<float> (parameters_.principal_point_x))
        * pt.z * static_cast<float> (constant_x);
      pt.y = (static_cast<float> (v) - static_cast<float> (parameters_.principal_point_y))
        * pt.z * static_cast<float> (constant_y);
    }
  }
  cloud.sensor_origin_.setZero ();
  cloud.sensor_orientation_.w () = 1.0f;
  cloud.sensor_orientation_.x () = 0.0f;
  cloud.sensor_orientation_.y () = 0.0f;
  cloud.sensor_orientation_.z () = 0.0f;
  return (true);
}


template <typename PointT> bool
LZFDepth16ImageReader::readOMP (const std::string &filename,
                                pcl::PointCloud<PointT> &cloud,
                                unsigned int num_threads)
{
  std::uint32_t uncompressed_size;
  std::vector<char> compressed_data;
  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
  {
    PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
    return (false);
  }

  if (uncompressed_size != getWidth () * getHeight () * 2)
  {
    PCL_DEBUG ("[pcl::io::LZFDepth16ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFDepth16ImageReader::read] Are you sure %s is a 16-bit depth PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight () * 2, filename.c_str (), getImageType ().c_str ());
    return (false);
  }

  std::vector<char> uncompressed_data (uncompressed_size);
  decompress (compressed_data, uncompressed_data);

  if (uncompressed_data.empty ())
  {
    PCL_ERROR ("[pcl::io::LZFDepth16ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
    return (false);
  }

  // Copy to PointT
  cloud.width    = getWidth ();
  cloud.height   = getHeight ();
  cloud.is_dense = true;
  cloud.resize (getWidth () * getHeight ());
  double constant_x = 1.0 / parameters_.focal_length_x,
         constant_y = 1.0 / parameters_.focal_length_y;
#ifdef _OPENMP
#pragma omp parallel for                                   \
  default(none)                                            \
  shared(cloud, constant_x, constant_y, uncompressed_data) \
  num_threads(num_threads)
#else
  pcl::utils::ignore(num_threads); // suppress warning if OMP is not present
#endif
  for (int i = 0; i < static_cast< int> (cloud.size ()); ++i)
  {
    int u = i % cloud.width;
    int v = i / cloud.width;
    PointT &pt = cloud[i];
    int depth_idx = 2*i;
    unsigned short val;
    memcpy (&val, &uncompressed_data[depth_idx], sizeof (unsigned short));
    if (val == 0)
    {
      pt.x = pt.y = pt.z = std::numeric_limits<float>::quiet_NaN ();
      if (cloud.is_dense)
      {
#pragma omp critical
        {
          if (cloud.is_dense)
            cloud.is_dense = false;
        }
      }
      continue;
    }

    pt.z = static_cast<float> (val * z_multiplication_factor_);
    pt.x = (static_cast<float> (u) - static_cast<float> (parameters_.principal_point_x)) 
      * pt.z * static_cast<float> (constant_x);
    pt.y = (static_cast<float> (v) - static_cast<float> (parameters_.principal_point_y)) 
      * pt.z * static_cast<float> (constant_y);
  }

  cloud.sensor_origin_.setZero ();
  cloud.sensor_orientation_.w () = 1.0f;
  cloud.sensor_orientation_.x () = 0.0f;
  cloud.sensor_orientation_.y () = 0.0f;
  cloud.sensor_orientation_.z () = 0.0f;
  return (true);
}


template <typename PointT> bool
LZFRGB24ImageReader::read (
    const std::string &filename, pcl::PointCloud<PointT> &cloud)
{
  std::uint32_t uncompressed_size;
  std::vector<char> compressed_data;
  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
  {
    PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
    return (false);
  }

  if (uncompressed_size != getWidth () * getHeight () * 3)
  {
    PCL_DEBUG ("[pcl::io::LZFRGB24ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFRGB24ImageReader::read] Are you sure %s is a 24-bit RGB PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight () * 3, filename.c_str (), getImageType ().c_str ());
    return (false);
  }

  std::vector<char> uncompressed_data (uncompressed_size);
  decompress (compressed_data, uncompressed_data);

  if (uncompressed_data.empty ())
  {
    PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
    return (false);
  }

  // Copy to PointT
  cloud.width  = getWidth ();
  cloud.height = getHeight ();
  cloud.resize (getWidth () * getHeight ());

  int rgb_idx = 0;
  unsigned char *color_r = reinterpret_cast<unsigned char*> (&uncompressed_data[0]);
  unsigned char *color_g = reinterpret_cast<unsigned char*> (&uncompressed_data[getWidth () * getHeight ()]);
  unsigned char *color_b = reinterpret_cast<unsigned char*> (&uncompressed_data[2 * getWidth () * getHeight ()]);

  for (std::size_t i = 0; i < cloud.size (); ++i, ++rgb_idx)
  {
    PointT &pt = cloud[i];

    pt.b = color_b[rgb_idx];
    pt.g = color_g[rgb_idx];
    pt.r = color_r[rgb_idx];
  }
  return (true);
}


template <typename PointT> bool
LZFRGB24ImageReader::readOMP (
    const std::string &filename, pcl::PointCloud<PointT> &cloud, unsigned int num_threads)
{
  std::uint32_t uncompressed_size;
  std::vector<char> compressed_data;
  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
  {
    PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
    return (false);
  }

  if (uncompressed_size != getWidth () * getHeight () * 3)
  {
    PCL_DEBUG ("[pcl::io::LZFRGB24ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFRGB24ImageReader::read] Are you sure %s is a 24-bit RGB PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight () * 3, filename.c_str (), getImageType ().c_str ());
    return (false);
  }

  std::vector<char> uncompressed_data (uncompressed_size);
  decompress (compressed_data, uncompressed_data);

  if (uncompressed_data.empty ())
  {
    PCL_ERROR ("[pcl::io::LZFRGB24ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
    return (false);
  }

  // Copy to PointT
  cloud.width  = getWidth ();
  cloud.height = getHeight ();
  cloud.resize (getWidth () * getHeight ());

  unsigned char *color_r = reinterpret_cast<unsigned char*> (&uncompressed_data[0]);
  unsigned char *color_g = reinterpret_cast<unsigned char*> (&uncompressed_data[getWidth () * getHeight ()]);
  unsigned char *color_b = reinterpret_cast<unsigned char*> (&uncompressed_data[2 * getWidth () * getHeight ()]);

#ifdef _OPENMP
#pragma omp parallel for                   \
  default(none)                            \
  shared(cloud, color_b, color_g, color_r) \
  num_threads(num_threads)
#else
  pcl::utils::ignore(num_threads); // suppress warning if OMP is not present
#endif//_OPENMP
  for (long int i = 0; i < cloud.size (); ++i)
  {
    PointT &pt = cloud[i];

    pt.b = color_b[i];
    pt.g = color_g[i];
    pt.r = color_r[i];
  }
  return (true);
}


template <typename PointT> bool
LZFYUV422ImageReader::read (
    const std::string &filename, pcl::PointCloud<PointT> &cloud)
{
  std::uint32_t uncompressed_size;
  std::vector<char> compressed_data;
  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
  {
    PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
    return (false);
  }

  if (uncompressed_size != getWidth () * getHeight () * 2)
  {
    PCL_DEBUG ("[pcl::io::LZFYUV422ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFYUV422ImageReader::read] Are you sure %s is a 16-bit YUV422 PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight (), filename.c_str (), getImageType ().c_str ());
    return (false);
  }

  std::vector<char> uncompressed_data (uncompressed_size);
  decompress (compressed_data, uncompressed_data);

  if (uncompressed_data.empty ())
  {
    PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
    return (false);
  }

  // Convert YUV422 to RGB24 and copy to PointT
  cloud.width  = getWidth ();
  cloud.height = getHeight ();
  cloud.resize (getWidth () * getHeight ());

  int wh2 = getWidth () * getHeight () / 2;
  unsigned char *color_u = reinterpret_cast<unsigned char*> (&uncompressed_data[0]);
  unsigned char *color_y = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2]);
  unsigned char *color_v = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2 + getWidth () * getHeight ()]);

  int y_idx = 0;
  for (int i = 0; i < wh2; ++i, y_idx += 2)
  {
    int v = color_v[i] - 128;
    int u = color_u[i] - 128;

    PointT &pt1 = cloud[y_idx + 0];
    pt1.r =  CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
    pt1.g =  CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
    pt1.b =  CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));

    PointT &pt2 = cloud[y_idx + 1];
    pt2.r =  CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
    pt2.g =  CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
    pt2.b =  CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
  }

  return (true);
}


template <typename PointT> bool
LZFYUV422ImageReader::readOMP (
    const std::string &filename, pcl::PointCloud<PointT> &cloud, unsigned int num_threads)
{
  std::uint32_t uncompressed_size;
  std::vector<char> compressed_data;
  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
  {
    PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
    return (false);
  }

  if (uncompressed_size != getWidth () * getHeight () * 2)
  {
    PCL_DEBUG ("[pcl::io::LZFYUV422ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFYUV422ImageReader::read] Are you sure %s is a 16-bit YUV422 PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight (), filename.c_str (), getImageType ().c_str ());
    return (false);
  }

  std::vector<char> uncompressed_data (uncompressed_size);
  decompress (compressed_data, uncompressed_data);

  if (uncompressed_data.empty ())
  {
    PCL_ERROR ("[pcl::io::LZFYUV422ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
    return (false);
  }

  // Convert YUV422 to RGB24 and copy to PointT
  cloud.width  = getWidth ();
  cloud.height = getHeight ();
  cloud.resize (getWidth () * getHeight ());

  int wh2 = getWidth () * getHeight () / 2;
  unsigned char *color_u = reinterpret_cast<unsigned char*> (&uncompressed_data[0]);
  unsigned char *color_y = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2]);
  unsigned char *color_v = reinterpret_cast<unsigned char*> (&uncompressed_data[wh2 + getWidth () * getHeight ()]);

#ifdef _OPENMP
#pragma omp parallel for                        \
  default(none)                                 \
  shared(cloud, color_u, color_v, color_y, wh2) \
  num_threads(num_threads)
#else
  pcl::utils::ignore(num_threads); //suppress warning if OMP is not present
#endif//_OPENMP
  for (int i = 0; i < wh2; ++i)
  {
    int y_idx = 2*i;
    int v = color_v[i] - 128;
    int u = color_u[i] - 128;

    PointT &pt1 = cloud[y_idx + 0];
    pt1.r =  CLIP_CHAR (color_y[y_idx + 0] + ((v * 18678 + 8192 ) >> 14));
    pt1.g =  CLIP_CHAR (color_y[y_idx + 0] + ((v * -9519 - u * 6472 + 8192) >> 14));
    pt1.b =  CLIP_CHAR (color_y[y_idx + 0] + ((u * 33292 + 8192 ) >> 14));

    PointT &pt2 = cloud[y_idx + 1];
    pt2.r =  CLIP_CHAR (color_y[y_idx + 1] + ((v * 18678 + 8192 ) >> 14));
    pt2.g =  CLIP_CHAR (color_y[y_idx + 1] + ((v * -9519 - u * 6472 + 8192) >> 14));
    pt2.b =  CLIP_CHAR (color_y[y_idx + 1] + ((u * 33292 + 8192 ) >> 14));
  }

  return (true);
}


template <typename PointT> bool
LZFBayer8ImageReader::read (
    const std::string &filename, pcl::PointCloud<PointT> &cloud)
{
  std::uint32_t uncompressed_size;
  std::vector<char> compressed_data;
  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
  {
    PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
    return (false);
  }

  if (uncompressed_size != getWidth () * getHeight ())
  {
    PCL_DEBUG ("[pcl::io::LZFBayer8ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFBayer8ImageReader::read] Are you sure %s is a 8-bit Bayer PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight (), filename.c_str (), getImageType ().c_str ());
    return (false);
  }

  std::vector<char> uncompressed_data (uncompressed_size);
  decompress (compressed_data, uncompressed_data);

  if (uncompressed_data.empty ())
  {
    PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
    return (false);
  }

  // Convert Bayer8 to RGB24
  std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
  DeBayer i;
  i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
                     static_cast<unsigned char*> (&rgb_buffer[0]),
                     getWidth (), getHeight ());
  // Copy to PointT
  cloud.width  = getWidth ();
  cloud.height = getHeight ();
  cloud.resize (getWidth () * getHeight ());
  int rgb_idx = 0;
  for (std::size_t i = 0; i < cloud.size (); ++i, rgb_idx += 3)
  {
    PointT &pt = cloud[i];

    pt.b = rgb_buffer[rgb_idx + 2];
    pt.g = rgb_buffer[rgb_idx + 1];
    pt.r = rgb_buffer[rgb_idx + 0];
  }
  return (true);
}


template <typename PointT> bool
LZFBayer8ImageReader::readOMP (
    const std::string &filename, pcl::PointCloud<PointT> &cloud, unsigned int num_threads)
{
  std::uint32_t uncompressed_size;
  std::vector<char> compressed_data;
  if (!loadImageBlob (filename, compressed_data, uncompressed_size))
  {
    PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Unable to read image data from %s.\n", filename.c_str ());
    return (false);
  }

  if (uncompressed_size != getWidth () * getHeight ())
  {
    PCL_DEBUG ("[pcl::io::LZFBayer8ImageReader::read] Uncompressed data has wrong size (%u), while in fact it should be %u bytes. \n[pcl::io::LZFBayer8ImageReader::read] Are you sure %s is a 8-bit Bayer PCLZF file? Identifier says: %s\n", uncompressed_size, getWidth () * getHeight (), filename.c_str (), getImageType ().c_str ());
    return (false);
  }

  std::vector<char> uncompressed_data (uncompressed_size);
  decompress (compressed_data, uncompressed_data);

  if (uncompressed_data.empty ())
  {
    PCL_ERROR ("[pcl::io::LZFBayer8ImageReader::read] Error uncompressing data stored in %s!\n", filename.c_str ());
    return (false);
  }

  // Convert Bayer8 to RGB24
  std::vector<unsigned char> rgb_buffer (getWidth () * getHeight () * 3);
  DeBayer i;
  i.debayerEdgeAware (reinterpret_cast<unsigned char*> (&uncompressed_data[0]),
                     static_cast<unsigned char*> (&rgb_buffer[0]),
                     getWidth (), getHeight ());
  // Copy to PointT
  cloud.width  = getWidth ();
  cloud.height = getHeight ();
  cloud.resize (getWidth () * getHeight ());
#ifdef _OPENMP
#pragma omp parallel for \
  default(none)          \
  num_threads(num_threads)
#else
  pcl::utils::ignore(num_threads); //suppress warning if OMP is not present
#endif//_OPENMP
  for (long int i = 0; i < cloud.size (); ++i)
  {
    PointT &pt = cloud[i];
    long int rgb_idx = 3*i;
    pt.b = rgb_buffer[rgb_idx + 2];
    pt.g = rgb_buffer[rgb_idx + 1];
    pt.r = rgb_buffer[rgb_idx + 0];
  }
  return (true);
}

} // namespace io
} // namespace pcl

#endif  //#ifndef PCL_LZF_IMAGE_IO_HPP_