// Copyright (c) Facebook, Inc. and its affiliates. All Rights Reserved.
#include <ATen/ATen.h>
#include <ATen/cuda/CUDAContext.h>
#include <ATen/cuda/Atomic.cuh>
#include <ATen/cuda/DeviceUtils.cuh>
#include <ATen/ceil_div.h>

// TODO make it in a common file
#define CUDA_1D_KERNEL_LOOP(i, n)                            \
  for (int i = blockIdx.x * blockDim.x + threadIdx.x; i < n; \
       i += blockDim.x * gridDim.x)


template <typename T>
__device__ T bilinear_interpolate_3d(const T* bottom_data,
    const int depth, const int height, const int width,
    T z, T y, T x,
    const int index /* index for debug only*/) {

  // deal with cases that inverse elements are out of feature map boundary
  if (z < -1.0 || z > depth || y < -1.0 || y > height || x < -1.0 || x > width) {
    //empty
    return 0;
  }

  if (z <= 0) z = 0;
  if (y <= 0) y = 0;
  if (x <= 0) x = 0;

  int z_low = (int) z;
  int y_low = (int) y;
  int x_low = (int) x;
  int z_high;
  int y_high;
  int x_high;

  if (z_low >= depth - 1) {
    z_high = z_low = depth - 1;
    z = (T) z_low;
  } else {
    z_high = z_low + 1;
  }

  if (y_low >= height - 1) {
    y_high = y_low = height - 1;
    y = (T) y_low;
  } else {
    y_high = y_low + 1;
  }

  if (x_low >= width - 1) {
    x_high = x_low = width - 1;
    x = (T) x_low;
  } else {
    x_high = x_low + 1;
  }

  T lz = z - z_low;
  T ly = y - y_low;
  T lx = x - x_low;
  T hz = 1. - lz, hy = 1. - ly, hx = 1. - lx;
  // do bilinear interpolation

  // T v1 = bottom_data[y_low * width + x_low];
  // T v2 = bottom_data[y_low * width + x_high];
  // T v3 = bottom_data[y_high * width + x_low];
  // T v4 = bottom_data[y_high * width + x_high];

  // T w1 = hy * hx, w2 = hy * lx, w3 = ly * hx, w4 = ly * lx;
  // T val = (w1 * v1 + w2 * v2 + w3 * v3 + w4 * v4);


  T v1 = bottom_data[z_low * depth + y_low * width + x_low];
  T v2 = bottom_data[z_low * depth + y_low * width + x_high];

  T v3 = bottom_data[z_low * depth + y_high * width + x_low];
  T v4 = bottom_data[z_low * depth + y_high * width + x_high];

  T v5 = bottom_data[z_high * depth + y_low * width + x_low];
  T v6 = bottom_data[z_high * depth + y_low * width + x_high];

  T v7 = bottom_data[z_high * depth + y_high * width + x_low];
  T v8 = bottom_data[z_high * depth + y_high * width + x_high];

  T w1 = hz * hy * hx, w2 = hz * hy * lx, w3 = hz * ly * hx, w4 = hz * ly * lx, w5 = lz * hy * hx, w6 = lz * hy * lx, w7 = lz * ly * hx, w8 = lz * ly * lx;

  T val = (w1 * v1 + w2 * v2 + w3 * v3 + w4 * v4 + w5 * v5 + w6 * v6 + w7 * v7 + w8 * v8);

  return val;
}

template <typename T>
__global__ void RoIAlign3DForward(const int nthreads, const T* bottom_data,
    const T spatial_scale, const int channels,
    const int depth, const int height, const int width,
    const int pooled_depth, const int pooled_height, const int pooled_width,
    const int sampling_ratio,
    const T* bottom_rois, T* top_data) {
  CUDA_1D_KERNEL_LOOP(index, nthreads) {
    // (n, c, ph, pw, pd) is an element in the pooled output
    int pw = index % pooled_width;                                      // width index
    int ph = (index / pooled_width) % pooled_height;                    // height index
    int pd = (index / pooled_width / pooled_height) % pooled_depth;     // depth index
    int c = (index / pooled_width / pooled_height / pooled_depth) % channels;          // channel index
    int n = index / pooled_width / pooled_height / pooled_depth / channels;            // batch index

    const T* offset_bottom_rois = bottom_rois + n * 7;
    int roi_batch_ind = offset_bottom_rois[0];

    // Do not using rounding; this implementation detail is critical
    T roi_start_w = offset_bottom_rois[1] * spatial_scale;
    T roi_start_h = offset_bottom_rois[2] * spatial_scale;
    T roi_end_w = offset_bottom_rois[3] * spatial_scale;
    T roi_end_h = offset_bottom_rois[4] * spatial_scale;
    T roi_start_d = offset_bottom_rois[5] * spatial_scale;
    T roi_end_d = offset_bottom_rois[6] * spatial_scale;
    // T roi_start_w = round(offset_bottom_rois[1] * spatial_scale);
    // T roi_start_h = round(offset_bottom_rois[2] * spatial_scale);
    // T roi_end_w = round(offset_bottom_rois[3] * spatial_scale);
    // T roi_end_h = round(offset_bottom_rois[4] * spatial_scale);

    // Force malformed ROIs to be 1x1
    T roi_width = max(roi_end_w - roi_start_w, (T)1.);
    T roi_height = max(roi_end_h - roi_start_h, (T)1.);
    T roi_depth = max(roi_end_d - roi_start_d, (T)1.);
    T bin_size_d = static_cast<T>(roi_depth) / static_cast<T>(pooled_depth);
    T bin_size_h = static_cast<T>(roi_height) / static_cast<T>(pooled_height);
    T bin_size_w = static_cast<T>(roi_width) / static_cast<T>(pooled_width);

    const T* offset_bottom_data = bottom_data + (roi_batch_ind * channels + c) * depth * height * width;

    // We use roi_bin_grid to sample the grid and mimic integral
    int roi_bin_grid_d = (sampling_ratio > 0) ? sampling_ratio : ceil(roi_depth / pooled_depth); // e.g., = 2
    int roi_bin_grid_h = (sampling_ratio > 0) ? sampling_ratio : ceil(roi_height / pooled_height); 
    int roi_bin_grid_w = (sampling_ratio > 0) ? sampling_ratio : ceil(roi_width / pooled_width);

    // We do average (integral) pooling inside a bin
    const T count = roi_bin_grid_d * roi_bin_grid_h * roi_bin_grid_w; // e.g. = 8

    T output_val = 0.;
    for (int iz = 0; iz < roi_bin_grid_d; iz ++) // e.g., iy = 0, 1
    {
      const T z = roi_start_d + pd * bin_size_d + static_cast<T>(iz + .5f) * bin_size_d / static_cast<T>(roi_bin_grid_d);// e.g., 0.5, 1.5
      for (int iy = 0; iy < roi_bin_grid_h; iy ++)
      {
        const T y = roi_start_h + ph * bin_size_h + static_cast<T>(iy + .5f) * bin_size_h / static_cast<T>(roi_bin_grid_h); 
        for (int ix = 0; ix < roi_bin_grid_w; ix ++)
        {
          const T x = roi_start_w + pw * bin_size_w + static_cast<T>(ix + .5f) * bin_size_w / static_cast<T>(roi_bin_grid_w);

          T val = bilinear_interpolate_3d(offset_bottom_data, depth, height, width, z, y, x, index);
          output_val += val;
        }
      }
    }
    output_val /= count;

    top_data[index] = output_val;
  }
}


template <typename T>
__device__ void bilinear_interpolate_3d_gradient(
    const int depth, const int height, const int width,
    T z, T y, T x,
    T & w1, T & w2, T & w3, T & w4, T & w5, T & w6, T & w7, T & w8,
    int & x_low, int & x_high, int & y_low, int & y_high, int & z_low, int & z_high,
    const int index /* index for debug only*/) {

  // deal with cases that inverse elements are out of feature map boundary
  if (z < -1.0 || z > depth || y < -1.0 || y > height || x < -1.0 || x > width) {
    //empty
    w1 = w2 = w3 = w4 = w5 = w6 = w7 = w8 = 0.;
    x_low = x_high = y_low = y_high = z_low = z_high = -1;
    return;
  }

  if (z <= 0) z = 0;
  if (y <= 0) y = 0;
  if (x <= 0) x = 0;

  z_low = (int) z;
  y_low = (int) y;
  x_low = (int) x;

  if (z_low >= depth - 1) {
    z_high = z_low = depth - 1;
    z = (T) z_low;
  } else {
    z_high = z_low + 1;
  }

  if (y_low >= height - 1) {
    y_high = y_low = height - 1;
    y = (T) y_low;
  } else {
    y_high = y_low + 1;
  }

  if (x_low >= width - 1) {
    x_high = x_low = width - 1;
    x = (T) x_low;
  } else {
    x_high = x_low + 1;
  }

  T lz = z - z_low;
  T ly = y - y_low;
  T lx = x - x_low;
  T hz = 1. - lz, hy = 1. - ly, hx = 1. - lx;

  // reference in forward
  // T v1 = bottom_data[y_low * width + x_low];
  // T v2 = bottom_data[y_low * width + x_high];
  // T v3 = bottom_data[y_high * width + x_low];
  // T v4 = bottom_data[y_high * width + x_high];
  // T val = (w1 * v1 + w2 * v2 + w3 * v3 + w4 * v4);

  w1 = hz * hy * hx, w2 = hz * hy * lx, w3 = hz * ly * hx, w4 = hz * ly * lx, w5 = lz * hy * hx, w6 = lz * hy * lx, w7 = lz * ly * hx, w8 = lz * ly * lx;

  return;
}

template <typename T>
__global__ void RoIAlign3DBackwardFeature(const int nthreads, const T* top_diff,
    const int num_rois, const T spatial_scale,
    const int channels, const int depth, const int height, const int width,
    const int pooled_depth, const int pooled_height, const int pooled_width,
    const int sampling_ratio,
    T* bottom_diff,
    const T* bottom_rois) {
  CUDA_1D_KERNEL_LOOP(index, nthreads) {
    // (n, c, pd, ph, pw) is an element in the pooled output
    int pw = index % pooled_width;                                      // width index
    int ph = (index / pooled_width) % pooled_height;                    // height index
    int pd = (index / pooled_width / pooled_height) % pooled_depth;     // depth index
    int c = (index / pooled_width / pooled_height / pooled_depth) % channels;          // channel index
    int n = index / pooled_width / pooled_height / pooled_depth / channels;            // batch index

    const T* offset_bottom_rois = bottom_rois + n * 7;
    int roi_batch_ind = offset_bottom_rois[0];

    // Do not using rounding; this implementation detail is critical
    T roi_start_w = offset_bottom_rois[1] * spatial_scale;
    T roi_start_h = offset_bottom_rois[2] * spatial_scale;
    T roi_end_w = offset_bottom_rois[3] * spatial_scale;
    T roi_end_h = offset_bottom_rois[4] * spatial_scale;
    T roi_start_d = offset_bottom_rois[5] * spatial_scale;
    T roi_end_d = offset_bottom_rois[6] * spatial_scale;
    // T roi_start_w = round(offset_bottom_rois[1] * spatial_scale);
    // T roi_start_h = round(offset_bottom_rois[2] * spatial_scale);
    // T roi_end_w = round(offset_bottom_rois[3] * spatial_scale);
    // T roi_end_h = round(offset_bottom_rois[4] * spatial_scale);

    // Force malformed ROIs to be 1x1
    T roi_width = max(roi_end_w - roi_start_w, (T)1.);
    T roi_height = max(roi_end_h - roi_start_h, (T)1.);
    T roi_depth = max(roi_end_d - roi_start_d, (T)1.);
    T bin_size_d = static_cast<T>(roi_depth) / static_cast<T>(pooled_depth);
    T bin_size_h = static_cast<T>(roi_height) / static_cast<T>(pooled_height);
    T bin_size_w = static_cast<T>(roi_width) / static_cast<T>(pooled_width);

    T* offset_bottom_diff = bottom_diff + (roi_batch_ind * channels + c) * depth * height * width;

    int top_offset    = (n * channels + c) * pooled_depth * pooled_height * pooled_width;
    const T* offset_top_diff = top_diff + top_offset;
    const T top_diff_this_bin = offset_top_diff[pd * pooled_height * pooled_width + ph * pooled_width + pw];

    // We use roi_bin_grid to sample the grid and mimic integral
    int roi_bin_grid_d = (sampling_ratio > 0) ? sampling_ratio : ceil(roi_depth / pooled_depth); // e.g., = 2
    int roi_bin_grid_h = (sampling_ratio > 0) ? sampling_ratio : ceil(roi_height / pooled_height); 
    int roi_bin_grid_w = (sampling_ratio > 0) ? sampling_ratio : ceil(roi_width / pooled_width);

    // We do average (integral) pooling inside a bin
    const T count = roi_bin_grid_d * roi_bin_grid_h * roi_bin_grid_w; // e.g. = 8

    for (int iz = 0; iz < roi_bin_grid_d; iz ++) // e.g., iy = 0, 1
    {
      const T z = roi_start_d + pd * bin_size_d + static_cast<T>(iz + .5f) * bin_size_d / static_cast<T>(roi_bin_grid_d);// e.g., 0.5, 1.5
      for (int iy = 0; iy < roi_bin_grid_h; iy ++) 
      {
        const T y = roi_start_h + ph * bin_size_h + static_cast<T>(iy + .5f) * bin_size_h / static_cast<T>(roi_bin_grid_h);
        for (int ix = 0; ix < roi_bin_grid_w; ix ++)
        {
          const T x = roi_start_w + pw * bin_size_w + static_cast<T>(ix + .5f) * bin_size_w / static_cast<T>(roi_bin_grid_w);

          T w1, w2, w3, w4, w5, w6, w7, w8;
          int x_low, x_high, y_low, y_high, z_low, z_high;

          bilinear_interpolate_3d_gradient(depth, height, width, z, y, x,
              w1, w2, w3, w4,w5, w6, w7, w8,
              x_low, x_high, y_low, y_high,z_low, z_high,
              index);

          T g1 = top_diff_this_bin * w1 / count;
          T g2 = top_diff_this_bin * w2 / count;
          T g3 = top_diff_this_bin * w3 / count;
          T g4 = top_diff_this_bin * w4 / count;
          T g5 = top_diff_this_bin * w5 / count;
          T g6 = top_diff_this_bin * w6 / count;
          T g7 = top_diff_this_bin * w7 / count;
          T g8 = top_diff_this_bin * w8 / count;

          if (x_low >= 0 && x_high >= 0 && y_low >= 0 && y_high >= 0 && z_low >= 0 && z_high >= 0)
          {
            atomicAdd(offset_bottom_diff + z_low * depth + y_low * width + x_low, static_cast<T>(g1));
            atomicAdd(offset_bottom_diff + z_low * depth + y_low * width + x_high, static_cast<T>(g2));
            atomicAdd(offset_bottom_diff + z_low * depth + y_high * width + x_low, static_cast<T>(g3));
            atomicAdd(offset_bottom_diff + z_low * depth + y_high * width + x_high, static_cast<T>(g4));
            atomicAdd(offset_bottom_diff + z_high * depth + y_low * width + x_low, static_cast<T>(g5));
            atomicAdd(offset_bottom_diff + z_high * depth + y_low * width + x_high, static_cast<T>(g6));
            atomicAdd(offset_bottom_diff + z_high * depth + y_high * width + x_low, static_cast<T>(g7));
            atomicAdd(offset_bottom_diff + z_high * depth + y_high * width + x_high, static_cast<T>(g8));

          } // if
        } // ix
      } // iy
    } // iz
  } // CUDA_1D_KERNEL_LOOP
} // RoIAlignBackward


at::Tensor ROIAlign_3d_forward_cuda(const at::Tensor& input,
                                 const at::Tensor& rois,
                                 const float spatial_scale,
                                 const int pooled_depth,
                                 const int pooled_height,
                                 const int pooled_width,
                                 const int sampling_ratio) {
  AT_ASSERTM(input.type().is_cuda(), "input must be a CUDA tensor");
  AT_ASSERTM(rois.type().is_cuda(), "rois must be a CUDA tensor");

  auto num_rois = rois.size(0);
  auto channels = input.size(1);
  auto depth = input.size(2);
  auto height = input.size(3);
  auto width = input.size(4);

  auto output = at::empty({num_rois, channels, pooled_depth, pooled_height, pooled_width}, input.options());
  auto output_size = num_rois * pooled_depth * pooled_height * pooled_width * channels;
  cudaStream_t stream = at::cuda::getCurrentCUDAStream();

  dim3 grid(std::min(at::ceil_div(output_size, 512L), 4096L));
  dim3 block(512);

  if (output.numel() == 0) {
    AT_CUDA_CHECK(cudaGetLastError());
    return output;
  }

  AT_DISPATCH_FLOATING_TYPES(input.type(), "ROIAlign_3d_forward", [&] {
    RoIAlign3DForward<scalar_t><<<grid, block, 0, stream>>>(
         output_size,
         input.contiguous().data<scalar_t>(),
         spatial_scale,
         channels,
         depth,
         height,
         width,
         pooled_depth,
         pooled_height,
         pooled_width,
         sampling_ratio,
         rois.contiguous().data<scalar_t>(),
         output.data<scalar_t>());
  });
  AT_CUDA_CHECK(cudaGetLastError());
  return output;
}

// TODO remove the dependency on input and use instead its sizes -> save memory
at::Tensor ROIAlign_3d_backward_cuda(const at::Tensor& grad,
                                  const at::Tensor& rois,
                                  const float spatial_scale,
                                  const int pooled_depth,
                                  const int pooled_height,
                                  const int pooled_width,
                                  const int batch_size,
                                  const int channels,
                                  const int depth,
                                  const int height,
                                  const int width,
                                  const int sampling_ratio) {
  AT_ASSERTM(grad.type().is_cuda(), "grad must be a CUDA tensor");
  AT_ASSERTM(rois.type().is_cuda(), "rois must be a CUDA tensor");

  auto num_rois = rois.size(0);
  auto grad_input = at::zeros({batch_size, channels, depth, height, width}, grad.options());

  cudaStream_t stream = at::cuda::getCurrentCUDAStream();

  dim3 grid(std::min(at::ceil_div(grad.numel(), 512L), 4096L));
  dim3 block(512);

  // handle possibly empty gradients
  if (grad.numel() == 0) {
    AT_CUDA_CHECK(cudaGetLastError());
    return grad_input;
  }

  AT_DISPATCH_FLOATING_TYPES(grad.type(), "ROIAlign_3d_backward", [&] {
    RoIAlign3DBackwardFeature<scalar_t><<<grid, block, 0, stream>>>(
         grad.numel(),
         grad.contiguous().data<scalar_t>(),
         num_rois,
         spatial_scale,
         channels,
         depth,
         height,
         width,
         pooled_depth,
         pooled_height,
         pooled_width,
         sampling_ratio,
         grad_input.data<scalar_t>(),
         rois.contiguous().data<scalar_t>());
  });
  AT_CUDA_CHECK(cudaGetLastError());
  return grad_input;
}
