/*******************************************************************************
 * Copyright (c) 2021 Nerian Vision GmbH
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *******************************************************************************/

#ifndef VISIONTRANSFER_RECONSTRUCT3D_OPEN3D_H
#define VISIONTRANSFER_RECONSTRUCT3D_OPEN3D_H

#ifdef OPEN3D_VERSION

namespace visiontransfer {

/*
 * Open3d-specific implementations that need to be inlined in order to avoid
 * dependencies for projects that do not make use of Open3D
 */

inline std::shared_ptr<open3d::geometry::PointCloud> Reconstruct3D::createOpen3DCloud(
        const ImageSet& imageSet, bool withColor, unsigned short minDisparity) {

    int numPoints = imageSet.getWidth() * imageSet.getHeight();
    std::shared_ptr<open3d::geometry::PointCloud> ret(new open3d::geometry::PointCloud());

    // Convert the 3D point cloud
    ret->points_.resize(numPoints);

    float* points = createPointMap(imageSet, minDisparity);
    float* end = &points[4*numPoints];
    Eigen::Vector3d* dest = &ret->points_[0];

    while(points != end) {
        float x = *(points++);
        float y = *(points++);
        float z = *(points++);
        points++;

        *dest = Eigen::Vector3d(x, y, z);
        dest++;
    }

    // Convert the color information if enabled
    if(withColor && imageSet.hasImageType(ImageSet::IMAGE_LEFT)) {
        ret->colors_.resize(numPoints);
        unsigned char* pixel = imageSet.getPixelData(ImageSet::IMAGE_LEFT);
        Eigen::Vector3d* color = &ret->colors_[0];
        Eigen::Vector3d* colorEnd = &ret->colors_[numPoints];

        switch(imageSet.getPixelFormat(ImageSet::IMAGE_LEFT)) {
            case ImageSet::FORMAT_8_BIT_MONO:
                while(color != colorEnd) {
                    double col = double(*(pixel++))/0xFF;
                    *(color++) = Eigen::Vector3d(col, col, col);
                }
                break;
            case ImageSet::FORMAT_12_BIT_MONO:
                while(color != colorEnd) {
                    double col = double(*reinterpret_cast<unsigned short*>(pixel))/0xFFF;
                    pixel+=2;
                    *(color++) = Eigen::Vector3d(col, col, col);
                }
                break;
            case ImageSet::FORMAT_8_BIT_RGB:
                while(color != colorEnd) {
                    double r = double(*(pixel++))/0xFF;
                    double g = double(*(pixel++))/0xFF;
                    double b = double(*(pixel++))/0xFF;
                    *(color++) = Eigen::Vector3d(r, g, b);
                }
                break;
            default: throw std::runtime_error("Illegal pixel format");
        }
    }

    return ret;
}

inline std::shared_ptr<open3d::geometry::RGBDImage> Reconstruct3D::createOpen3DImageRGBD(const ImageSet& imageSet,
    unsigned short minDisparity) {

    std::shared_ptr<open3d::geometry::RGBDImage> ret(new open3d::geometry::RGBDImage);

    // Convert depth map
    ret->depth_.width_ = imageSet.getWidth();
    ret->depth_.height_ = imageSet.getHeight();
    ret->depth_.num_of_channels_ = 1;
    ret->depth_.bytes_per_channel_ = sizeof(float);
    ret->depth_.data_.resize(ret->depth_.width_*ret->depth_.height_*ret->depth_.bytes_per_channel_);

    float* zMap = createZMap(imageSet, minDisparity);
    memcpy(&ret->depth_.data_[0], zMap, ret->depth_.data_.size());

    // Convert color
    ret->color_.width_ = imageSet.getWidth();
    ret->color_.height_ = imageSet.getHeight();
    ret->color_.num_of_channels_ = 3;
    ret->color_.bytes_per_channel_ = 1;
    ret->color_.data_.resize(ret->color_.width_ * ret->color_.height_ *
        ret->color_.num_of_channels_ * ret->color_.bytes_per_channel_);

    unsigned char* srcPixel = imageSet.getPixelData(ImageSet::IMAGE_LEFT);
    unsigned char* dstPixel = &ret->color_.data_[0];
    unsigned char* dstEnd = &ret->color_.data_[ret->color_.data_.size()];

    switch(imageSet.getPixelFormat(ImageSet::IMAGE_LEFT)) {
        case ImageSet::FORMAT_8_BIT_MONO:
            while(dstPixel != dstEnd) {
                *(dstPixel++) = *srcPixel;
                *(dstPixel++) = *srcPixel;
                *(dstPixel++) = *(srcPixel++);
            }
            break;
        case ImageSet::FORMAT_12_BIT_MONO:
            while(dstPixel != dstEnd) {
                unsigned short pixel16Bit = *reinterpret_cast<unsigned short*>(srcPixel);
                unsigned char pixel8Bit = pixel16Bit / 0xF;
                srcPixel += 2;

                *(dstPixel++) = pixel8Bit;
                *(dstPixel++) = pixel8Bit;
                *(dstPixel++) = pixel8Bit;
            }
            break;
        case ImageSet::FORMAT_8_BIT_RGB:
            memcpy(&ret->color_.data_[0], srcPixel, ret->color_.data_.size());
            break;
        default: throw std::runtime_error("Illegal pixel format");
    }

    return ret;
}

} // namespace

#endif
#endif