// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018-2021 www.open3d.org
//
// 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.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include "open3d/t/pipelines/slam/Model.h"

#include "open3d/core/Tensor.h"
#include "open3d/t/geometry/Image.h"
#include "open3d/t/geometry/RGBDImage.h"
#include "open3d/t/geometry/Utility.h"
#include "open3d/t/geometry/VoxelBlockGrid.h"
#include "open3d/t/pipelines/odometry/RGBDOdometry.h"
#include "open3d/t/pipelines/slam/Frame.h"

namespace open3d {
namespace t {
namespace pipelines {
namespace slam {

Model::Model(float voxel_size,
             int block_resolution,
             int est_block_count,
             const core::Tensor& T_init,
             const core::Device& device)
    : voxel_grid_(std::vector<std::string>({"tsdf", "weight", "color"}),
                  std::vector<core::Dtype>(
                          {core::Float32, core::UInt16, core::UInt16}),
                  std::vector<core::SizeVector>({{1}, {1}, {3}}),
                  voxel_size,
                  block_resolution,
                  est_block_count,
                  device),
      T_frame_to_world_(T_init.To(core::Device("CPU:0"))) {}

void Model::SynthesizeModelFrame(Frame& raycast_frame,
                                 float depth_scale,
                                 float depth_min,
                                 float depth_max,
                                 bool enable_color) {
    auto result = voxel_grid_.RayCast(
            frustum_block_coords_, raycast_frame.GetIntrinsics(),
            t::geometry::InverseTransformation(GetCurrentFramePose()),
            raycast_frame.GetWidth(), raycast_frame.GetHeight(),
            {"depth", "color"}, depth_scale, depth_min, depth_max,
            std::min(frame_id_ * 1.0f, 3.0f));
    raycast_frame.SetData("depth", result["depth"]);
    if (enable_color) {
        raycast_frame.SetData("color", result["color"]);
    } else if (raycast_frame.GetData("color").NumElements() == 0) {
        // Put a dummy RGB frame to enable RGBD odometry in TrackFrameToModel
        raycast_frame.SetData("color",
                              core::Tensor({raycast_frame.GetHeight(),
                                            raycast_frame.GetWidth(), 3},
                                           core::Float32, core::Device()));
    }
}

odometry::OdometryResult Model::TrackFrameToModel(const Frame& input_frame,
                                                  const Frame& raycast_frame,
                                                  float depth_scale,
                                                  float depth_max,
                                                  float depth_diff) {
    const static core::Tensor identity =
            core::Tensor::Eye(4, core::Float64, core::Device("CPU:0"));

    // TODO: more customized / optimized
    return odometry::RGBDOdometryMultiScale(
            t::geometry::RGBDImage(input_frame.GetDataAsImage("color"),
                                   input_frame.GetDataAsImage("depth")),
            t::geometry::RGBDImage(raycast_frame.GetDataAsImage("color"),
                                   raycast_frame.GetDataAsImage("depth")),
            raycast_frame.GetIntrinsics(), identity, depth_scale, depth_max,
            std::vector<odometry::OdometryConvergenceCriteria>{6, 3, 1},
            odometry::Method::PointToPlane,
            odometry::OdometryLossParams(depth_diff));
}

void Model::Integrate(const Frame& input_frame,
                      float depth_scale,
                      float depth_max) {
    t::geometry::Image depth = input_frame.GetDataAsImage("depth");
    t::geometry::Image color = input_frame.GetDataAsImage("color");
    core::Tensor intrinsic = input_frame.GetIntrinsics();
    core::Tensor extrinsic =
            t::geometry::InverseTransformation(GetCurrentFramePose());
    frustum_block_coords_ = voxel_grid_.GetUniqueBlockCoordinates(
            depth, intrinsic, extrinsic, depth_scale, depth_max);
    voxel_grid_.Integrate(frustum_block_coords_, depth, color, intrinsic,
                          extrinsic);
}

t::geometry::PointCloud Model::ExtractPointCloud(int estimated_number,
                                                 float weight_threshold) {
    return voxel_grid_.ExtractPointCloud(estimated_number, weight_threshold);
}

t::geometry::TriangleMesh Model::ExtractTriangleMesh(int estimated_number,
                                                     float weight_threshold) {
    return voxel_grid_.ExtractTriangleMesh(estimated_number, weight_threshold);
}

core::HashMap Model::GetHashMap() { return voxel_grid_.GetHashMap(); }

}  // namespace slam
}  // namespace pipelines
}  // namespace t
}  // namespace open3d
