/** These are NAIVE CUDA kerenels for laplacian smoothing of organized point clouds
 *  There are probably a few bugs in here. I am not as familiar with GPU programming but tried my best.
 *  I had a hard time debugging GPU programs so I kept everything as simple and verbose as possible. 
*/
#include <math.h>       /* ceil */
#include <iostream>


    #define EPS 1e-12f
    #define BLOCK_WIDTH 32
    #define MAX_DIST_DEFAULT 1000.0
    
    //////// Start Helper Functions /////////
    __device__ void LoadPoint(float* out, const float* in, int outIdx, int inIdx)
    {
        out[outIdx * 3 + 0] = in[inIdx * 3 + 0]; // x cordinate
        out[outIdx * 3 + 1] = in[inIdx * 3 + 1]; // y cordinate
        out[outIdx * 3 + 2] = in[inIdx * 3 + 2]; // z cordinate
    }
    
    __device__ void ScalePointInPlace(float* point, const float& scale)
    {
        point[0] = point[0] * scale;
        point[1] = point[1] * scale;
        point[2] = point[2] * scale;
    }
    
    __device__ void AddPointsInPlace(float* point1, const float* point2)
    {
        point1[0] = point1[0] + point2[0];
        point1[1] = point1[1] + point2[1];
        point1[2] = point1[2] + point2[2];
    }
    
    __device__ void SubtractPointsInPlace(float* point1, const float* point2)
    {
        point1[0] = point1[0] - point2[0];
        point1[1] = point1[1] - point2[1];
        point1[2] = point1[2] - point2[2];
    }
    
    __device__ float PointDistance(const float* point1, const float* point2)
    {
        float x3 = (point1[0] - point2[0]);
        float y3 = (point1[1] - point2[1]);
        float z3 = (point1[2] - point2[2]);
        return norm3df(x3, y3, z3);
    }
    
    __device__ void Print2DPointArray(float* point, int rows, int cols)
    {
        printf("Shared Memory Size: %d\n ", rows);
        for (int i = 0; i < rows; ++i)
        {
            for (int j = 0; j < cols; ++j)
            {
                int idx = i * cols + j;
                // printf("(%.2f,%.2f,%.2f), ", point[idx*3], point[idx*3 + 1], point[idx*3 + 2]);
                printf("(%.4f), ", point[idx * 3 + 1]);
            }
            printf("\n\n");
        }
    }
    
    __device__ void IntegeratePoint(int& nbr_shmIdx, float* SHM_POINTS, float* point_temp, float* point,
                                    float& total_weight, float* sum_point, float max_dist = MAX_DIST_DEFAULT)
    {
        LoadPoint(point_temp, SHM_POINTS, 0, nbr_shmIdx);
        float dist = PointDistance(point, point_temp);
        // branch divergence
        if (dist > max_dist || isnan(dist)) return;
        float weight = 1.0f / (dist + EPS);
        total_weight += weight;
        ScalePointInPlace(point_temp, weight);
        AddPointsInPlace(sum_point, point_temp);
    }
    //////// End Helper Functions /////////
    
    // K3 Constants
    #define KERNEL_SIZE_K3 3
    #define SHM_SIZE_K3 (BLOCK_WIDTH + KERNEL_SIZE_K3 - 1)
    #define HALF_RADIUS_K3 KERNEL_SIZE_K3 / 2
    
    
    //////////////////// K3 Functions /////////////////////////
    __device__ void ReadBlockAndHaloK3(float* SHM_POINTS, const float* opc, const int& shmRow_y, const int& shmCol_x,
                                       const int& srcRow_y, const int& srcCol_x, const int& cols)
    {
        // Copy Halo Cells, will have LOTS of branch divergence here
        int shmIdx = 0;
        int srcIdx_temp = 0;
        if (threadIdx.x == 0)
        {
            // Left Column, inner border
            shmIdx = shmRow_y * SHM_SIZE_K3 + (shmCol_x - 1);
            srcIdx_temp = srcRow_y * cols + (srcCol_x - 1);
            LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
            if (threadIdx.y == 0)
            {
                // Top Left Corner
                shmIdx = (shmRow_y - 1) * SHM_SIZE_K3 + (shmCol_x - 1);
                srcIdx_temp = (srcRow_y - 1) * cols + (srcCol_x - 1);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
            }
            if (threadIdx.y == (blockDim.y - 1))
            {
                // Bottom Left Corner
                shmIdx = (shmRow_y + 1) * SHM_SIZE_K3 + (shmCol_x - 1);
                srcIdx_temp = (srcRow_y + 1) * cols + (srcCol_x - 1);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
            }
        }
        if (threadIdx.x == (blockDim.x - 1))
        {
            // Right Column, inner border
            shmIdx = shmRow_y * SHM_SIZE_K3 + (shmCol_x + 1);
            srcIdx_temp = srcRow_y * cols + (srcCol_x + 1);
            LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
            if (threadIdx.y == 0)
            {
                // Top Right Corner
                shmIdx = (shmRow_y - 1) * SHM_SIZE_K3 + (shmCol_x + 1);
                srcIdx_temp = (srcRow_y - 1) * cols + (srcCol_x + 1);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
            }
            if (threadIdx.y == (blockDim.y - 1))
            {
                // Bottom Right Corner
                shmIdx = (shmRow_y + 1) * SHM_SIZE_K3 + (shmCol_x + 1);
                srcIdx_temp = (srcRow_y + 1) * cols + (srcCol_x + 1);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
            }
        }
        if (threadIdx.y == 0)
        {
            // Top Row
            shmIdx = (shmRow_y - 1) * SHM_SIZE_K3 + (shmCol_x);
            srcIdx_temp = (srcRow_y - 1) * cols + (srcCol_x);
            LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
            // printf("Top Row; Row,Col: %d,%d; Point: (%.4f, %.4f, %.4f); Top Point: (%.4f, %.4f, %.4f)\n", srcRow_y,
            // srcCol_x, point[0], point[1], point[2], point_temp[0], point_temp[1], point_temp[2]);
        }
        if (threadIdx.y == (blockDim.y - 1))
        {
            // Bottom Row
            shmIdx = (shmRow_y + 1) * SHM_SIZE_K3 + (shmCol_x);
            srcIdx_temp = (srcRow_y + 1) * cols + (srcCol_x);
            LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
        }
    }
    
    __device__ void SmoothPointK3(float* SHM_POINTS, const float* opc, float* opc_out, const int& srcIdx,
                                  const int& shmRow_y, const int& shmCol_x, const int& srcRow_y, const int& srcCol_x,
                                  const int& cols, const float& lambda)
    {
    
        int this_shmIdx = shmRow_y * SHM_SIZE_K3 + shmCol_x; // shm point index for this points row/col
    
        float point[3];                 // will contain x,y,z point of this row/col
        float point_temp[3];            // a temporary for a point in stencil
        float sum_point[3] = {0, 0, 0}; // the weighted sum of points in stencil
        float dist = 0.0;               // distance between point and neighbor
        float weight = 0.0;             // weighting for point
        float total_weight = 0.0;       // total weight for scaling new point
        int nbr_shmIdx = 0;             // nbr shared index
    
        LoadPoint(point, SHM_POINTS, 0, this_shmIdx);
    
        // I manually unwrapped the 3X3 Kernel loop below
        // Mostly because the center point should not occur in the kernel
        // and I didn't want an if statement for branch divergence
    
        //////// Left ////////////
        //////// Left Top ////////
        nbr_shmIdx = (shmRow_y - 1) * SHM_SIZE_K3 + (shmCol_x - 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Left Center ////////
        nbr_shmIdx = (shmRow_y)*SHM_SIZE_K3 + (shmCol_x - 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Left Bottom ////////
        nbr_shmIdx = (shmRow_y + 1) * SHM_SIZE_K3 + (shmCol_x - 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Center  ////////////
        //////// Center Top /////////
        nbr_shmIdx = (shmRow_y - 1) * SHM_SIZE_K3 + (shmCol_x);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Center Bottom /////////
        nbr_shmIdx = (shmRow_y + 1) * SHM_SIZE_K3 + (shmCol_x);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Right  ////////////
        //////// Right Top /////////
        nbr_shmIdx = (shmRow_y - 1) * SHM_SIZE_K3 + (shmCol_x + 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Right Mid /////////
        nbr_shmIdx = (shmRow_y)*SHM_SIZE_K3 + (shmCol_x + 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Right Bottom /////////
        nbr_shmIdx = (shmRow_y + 1) * SHM_SIZE_K3 + (shmCol_x + 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Combine into New Point /////////
        /// This is a very convoluted way of doing this -> opc_out(i, j) = point + lambda * (sum_point / total_weight -
        /// point);
        // if total_weight == 0.0 we have no updates
        if (total_weight <= 0.0) return;
    
        float new_scale = 1.0f / total_weight;
        ScalePointInPlace(sum_point, new_scale);
        SubtractPointsInPlace(sum_point, point);
        ScalePointInPlace(sum_point, lambda);
        AddPointsInPlace(sum_point, point);
    
        // store in global memory
        LoadPoint(opc_out, sum_point, srcIdx, 0);
    }
    
    __global__ void LaplacianLoopK3(float* opc, float* opc_out, int rows, int cols, float lambda)
    {
        __shared__ float SHM_POINTS[SHM_SIZE_K3 * 3 * SHM_SIZE_K3 * 3]; // block of 3D Points in shared memory
    
        int shmRow_y = threadIdx.y + HALF_RADIUS_K3;    // row of shared memory, interior
        int shmCol_x = threadIdx.x + HALF_RADIUS_K3;    // col of shared memory, interior
        int shmIdx = shmRow_y * SHM_SIZE_K3 + shmCol_x; // index into shared memory, interior
    
        int srcRow_y = blockIdx.y * blockDim.y + threadIdx.y; // row in global memory
        int srcCol_x = blockIdx.x * blockDim.x + threadIdx.x; // col in global memory
        int srcIdx = srcRow_y * cols + srcCol_x;              // idx in global memory
    
        LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx); // Copy global memory point (x,y,z) into shared memory
        if (srcRow_y > 0 && srcRow_y < (rows - 1) && srcCol_x > 0 && srcCol_x < (cols - 1))
        // Only perform smoothing in interior of image/opc, branch divergence occurs here
        {
            // Copy Halo Cells, will have LOTS of branch divergence here
            // After this call SHM_POINTS is completely filled
            ReadBlockAndHaloK3(SHM_POINTS, opc, shmRow_y, shmCol_x, srcRow_y, srcCol_x, cols);
            __syncthreads();
    
            ////// Smooth Point Operation //////
            SmoothPointK3(SHM_POINTS, opc, opc_out, srcIdx, shmRow_y, shmCol_x, srcRow_y, srcCol_x, cols, lambda);
        }
    }
    
    ///////////////////////////////////////////////////////////////////
    /////////////////////// K5 Functions //////////////////////////////
    ///////////////////////////////////////////////////////////////////
    
    // K5 Constants
    #define KERNEL_SIZE_K5 5
    #define SHM_SIZE_K5 (BLOCK_WIDTH + KERNEL_SIZE_K5 - 1)
    #define HALF_RADIUS_K5 KERNEL_SIZE_K5 / 2
    
    
    __device__ void ReadBlockAndHaloK5(float* SHM_POINTS, const float* opc, const int& shmRow_y, const int& shmCol_x,
                                       const int& srcRow_y, const int& srcCol_x, const int& cols)
    {
        // Copy Halo Cells, will have LOTS of branch divergence here
        int shmIdx = 0;
        int srcIdx_temp = 0;
        if (threadIdx.x == 0)
        {
            // Left Column, inner border
            shmIdx = shmRow_y * SHM_SIZE_K5 + (shmCol_x - 1);
            srcIdx_temp = srcRow_y * cols + (srcCol_x - 1);
            LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
            shmIdx = shmRow_y * SHM_SIZE_K5 + (shmCol_x - 2);
            srcIdx_temp = srcRow_y * cols + (srcCol_x - 2);
            LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
            if (threadIdx.y == 0)
            {
                // Top Left Corner
                shmIdx = (shmRow_y - 1) * SHM_SIZE_K5 + (shmCol_x - 1);
                srcIdx_temp = (srcRow_y - 1) * cols + (srcCol_x - 1);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
                shmIdx = (shmRow_y - 2) * SHM_SIZE_K5 + (shmCol_x - 2);
                srcIdx_temp = (srcRow_y - 2) * cols + (srcCol_x - 2);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
                // 1 Up, 2 Left
                shmIdx = (shmRow_y - 1) * SHM_SIZE_K5 + (shmCol_x - 2);
                srcIdx_temp = (srcRow_y - 1) * cols + (srcCol_x - 2);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
                // 2 Up, 1 Left
                shmIdx = (shmRow_y - 2) * SHM_SIZE_K5 + (shmCol_x - 1);
                srcIdx_temp = (srcRow_y - 2) * cols + (srcCol_x - 1);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
            }
            if (threadIdx.y == (blockDim.y - 1))
            {
                // Bottom Left Corner
                shmIdx = (shmRow_y + 1) * SHM_SIZE_K5 + (shmCol_x - 1);
                srcIdx_temp = (srcRow_y + 1) * cols + (srcCol_x - 1);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
                shmIdx = (shmRow_y + 2) * SHM_SIZE_K5 + (shmCol_x - 2);
                srcIdx_temp = (srcRow_y + 2) * cols + (srcCol_x - 2);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
                // 1 Down, 2 Left
                shmIdx = (shmRow_y + 1) * SHM_SIZE_K5 + (shmCol_x - 2);
                srcIdx_temp = (srcRow_y + 1) * cols + (srcCol_x - 2);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
                // 2 Down, 1 left
                shmIdx = (shmRow_y + 2) * SHM_SIZE_K5 + (shmCol_x - 1);
                srcIdx_temp = (srcRow_y + 2) * cols + (srcCol_x - 1);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
            }
        }
        if (threadIdx.x == (blockDim.x - 1))
        {
            // Right Column, inner border
            shmIdx = shmRow_y * SHM_SIZE_K5 + (shmCol_x + 1);
            srcIdx_temp = srcRow_y * cols + (srcCol_x + 1);
            LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
            shmIdx = shmRow_y * SHM_SIZE_K5 + (shmCol_x + 2);
            srcIdx_temp = srcRow_y * cols + (srcCol_x + 2);
            LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
            if (threadIdx.y == 0)
            {
                // Top Right Corner
                shmIdx = (shmRow_y - 1) * SHM_SIZE_K5 + (shmCol_x + 1);
                srcIdx_temp = (srcRow_y - 1) * cols + (srcCol_x + 1);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
                shmIdx = (shmRow_y - 2) * SHM_SIZE_K5 + (shmCol_x + 2);
                srcIdx_temp = (srcRow_y - 2) * cols + (srcCol_x + 2);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
                // 1 Up, 2 Right
                shmIdx = (shmRow_y - 1) * SHM_SIZE_K5 + (shmCol_x + 2);
                srcIdx_temp = (srcRow_y - 1) * cols + (srcCol_x + 2);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
                // 2 Up, 1 Right
                shmIdx = (shmRow_y - 2) * SHM_SIZE_K5 + (shmCol_x + 1);
                srcIdx_temp = (srcRow_y - 2) * cols + (srcCol_x + 1);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
            }
            if (threadIdx.y == (blockDim.y - 1))
            {
                // Bottom Right Corner
                shmIdx = (shmRow_y + 1) * SHM_SIZE_K5 + (shmCol_x + 1);
                srcIdx_temp = (srcRow_y + 1) * cols + (srcCol_x + 1);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
                shmIdx = (shmRow_y + 2) * SHM_SIZE_K5 + (shmCol_x + 2);
                srcIdx_temp = (srcRow_y + 2) * cols + (srcCol_x + 2);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
                // 1 Down, 2 Right
                shmIdx = (shmRow_y + 1) * SHM_SIZE_K5 + (shmCol_x + 2);
                srcIdx_temp = (srcRow_y + 1) * cols + (srcCol_x + 2);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
                // 2 Down, 1 Right
                shmIdx = (shmRow_y + 2) * SHM_SIZE_K5 + (shmCol_x + 1);
                srcIdx_temp = (srcRow_y + 2) * cols + (srcCol_x + 1);
                LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
            }
        }
        if (threadIdx.y == 0)
        {
            // Top Row
            shmIdx = (shmRow_y - 1) * SHM_SIZE_K5 + (shmCol_x);
            srcIdx_temp = (srcRow_y - 1) * cols + (srcCol_x);
            LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
            shmIdx = (shmRow_y - 2) * SHM_SIZE_K5 + (shmCol_x);
            srcIdx_temp = (srcRow_y - 2) * cols + (srcCol_x);
            LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
        }
        if (threadIdx.y == (blockDim.y - 1))
        {
            // Bottom Row
            shmIdx = (shmRow_y + 1) * SHM_SIZE_K5 + (shmCol_x);
            srcIdx_temp = (srcRow_y + 1) * cols + (srcCol_x);
            LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
    
            shmIdx = (shmRow_y + 2) * SHM_SIZE_K5 + (shmCol_x);
            srcIdx_temp = (srcRow_y + 2) * cols + (srcCol_x);
            LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx_temp);
        }
    }
    
    __device__ void SmoothPointK5(float* SHM_POINTS, const float* opc, float* opc_out, const int& srcIdx,
                                  const int& shmRow_y, const int& shmCol_x, const int& srcRow_y, const int& srcCol_x,
                                  const int& cols, const float& lambda)
    {
    
        int this_shmIdx = shmRow_y * SHM_SIZE_K5 + shmCol_x; // shm point index for this points row/col
    
        float point[3];                 // will contain x,y,z point of this row/col
        float point_temp[3];            // a temporary for a point in stencil
        float sum_point[3] = {0, 0, 0}; // the weighted sum of points in stencil
        float total_weight = 0.0;       // total weight for scaling new point
        int nbr_shmIdx = 0;             // nbr shared index
    
        LoadPoint(point, SHM_POINTS, 0, this_shmIdx);
    
        // I manually unwrapped the 3X3 Kernel loop below
        // Mostly because the center point should not occur in the kernel
        // and I didn't want an if statement for branch divergence
    
        //////// Left ////////////
        //////// Left Top ////////
        nbr_shmIdx = (shmRow_y - 1) * SHM_SIZE_K5 + (shmCol_x - 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Left Center ////////
        nbr_shmIdx = (shmRow_y)*SHM_SIZE_K5 + (shmCol_x - 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Left Bottom ////////
        nbr_shmIdx = (shmRow_y + 1) * SHM_SIZE_K5 + (shmCol_x - 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        /////// Far Left ///////////
        nbr_shmIdx = (shmRow_y - 2) * SHM_SIZE_K5 + (shmCol_x - 2);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        nbr_shmIdx = (shmRow_y - 1) * SHM_SIZE_K5 + (shmCol_x - 2);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        nbr_shmIdx = (shmRow_y - 0) * SHM_SIZE_K5 + (shmCol_x - 2);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        nbr_shmIdx = (shmRow_y + 1) * SHM_SIZE_K5 + (shmCol_x - 2);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        nbr_shmIdx = (shmRow_y + 2) * SHM_SIZE_K5 + (shmCol_x - 2);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //// Far Left Extras ////
        nbr_shmIdx = (shmRow_y - 2) * SHM_SIZE_K5 + (shmCol_x - 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        nbr_shmIdx = (shmRow_y + 2) * SHM_SIZE_K5 + (shmCol_x - 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Center  ////////////
        //////// Center Top /////////
        nbr_shmIdx = (shmRow_y - 1) * SHM_SIZE_K5 + (shmCol_x);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Center Bottom /////////
        nbr_shmIdx = (shmRow_y + 1) * SHM_SIZE_K5 + (shmCol_x);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        ///////// Far Center ////////
        nbr_shmIdx = (shmRow_y - 2) * SHM_SIZE_K5 + (shmCol_x);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        nbr_shmIdx = (shmRow_y + 2) * SHM_SIZE_K5 + (shmCol_x);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Right  ////////////
        //////// Right Top /////////
        nbr_shmIdx = (shmRow_y - 1) * SHM_SIZE_K5 + (shmCol_x + 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Right Mid /////////
        nbr_shmIdx = (shmRow_y)*SHM_SIZE_K5 + (shmCol_x + 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Right Bottom /////////
        nbr_shmIdx = (shmRow_y + 1) * SHM_SIZE_K5 + (shmCol_x + 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        /////// Far Rigth ///////////
        nbr_shmIdx = (shmRow_y - 2) * SHM_SIZE_K5 + (shmCol_x + 2);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        nbr_shmIdx = (shmRow_y - 1) * SHM_SIZE_K5 + (shmCol_x + 2);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        nbr_shmIdx = (shmRow_y - 0) * SHM_SIZE_K5 + (shmCol_x + 2);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        nbr_shmIdx = (shmRow_y + 1) * SHM_SIZE_K5 + (shmCol_x + 2);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        nbr_shmIdx = (shmRow_y + 2) * SHM_SIZE_K5 + (shmCol_x + 2);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //// Far Rigth Extras ////
        nbr_shmIdx = (shmRow_y - 2) * SHM_SIZE_K5 + (shmCol_x + 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        nbr_shmIdx = (shmRow_y + 2) * SHM_SIZE_K5 + (shmCol_x + 1);
        IntegeratePoint(nbr_shmIdx, SHM_POINTS, point_temp, point, total_weight, sum_point);
    
        //////// Combine into New Point /////////
        /// This is a very convoluted way of doing this -> opc_out(i, j) = point + lambda * (sum_point / total_weight -
        /// point);
        // if total_weight == 0.0 we have no updates
        if (total_weight == 0.0) return;
    
        float new_scale = 1.0f / total_weight;
        ScalePointInPlace(sum_point, new_scale);
        SubtractPointsInPlace(sum_point, point);
        ScalePointInPlace(sum_point, lambda);
        AddPointsInPlace(sum_point, point);
    
        // store in global memory
        LoadPoint(opc_out, sum_point, srcIdx, 0);
    }
    
    __global__ void LaplacianLoopK5(float* opc, float* opc_out, int rows, int cols, float lambda)
    {
        __shared__ float SHM_POINTS[SHM_SIZE_K5 * 3 * SHM_SIZE_K5]; // block of 3D Points in shared memory
    
        int shmRow_y = threadIdx.y + HALF_RADIUS_K5;    // row of shared memory, interior
        int shmCol_x = threadIdx.x + HALF_RADIUS_K5;    // col of shared memory, interior
        int shmIdx = shmRow_y * SHM_SIZE_K5 + shmCol_x; // index into shared memory, interior
    
        int srcRow_y = blockIdx.y * blockDim.y + threadIdx.y; // row in global memory
        int srcCol_x = blockIdx.x * blockDim.x + threadIdx.x; // col in global memory
        int srcIdx = srcRow_y * cols + srcCol_x;              // idx in global memory
    
        LoadPoint(SHM_POINTS, opc, shmIdx, srcIdx); // Copy global memory point (x,y,z) into shared memory
        if (srcRow_y >= HALF_RADIUS_K5 && srcRow_y < (rows - HALF_RADIUS_K5) && srcCol_x >= HALF_RADIUS_K5 &&
            srcCol_x < (cols - HALF_RADIUS_K5))
        // Only perform smoothing in interior of image/opc, branch divergence occurs here
        {
            // Copy Halo Cells, will have LOTS of branch divergence here
            // After this call SHM_POINTS is completely filled
            ReadBlockAndHaloK5(SHM_POINTS, opc, shmRow_y, shmCol_x, srcRow_y, srcCol_x, cols);
            __syncthreads();
    
            ////// Smooth Point Operation //////
            SmoothPointK5(SHM_POINTS, opc, opc_out, srcIdx, shmRow_y, shmCol_x, srcRow_y, srcCol_x, cols, lambda);
        }
    }

    int main()
    {

        const int ROWS = 500;
        const int COLS = 500;
        const float lambda = 1.0f;
        float *opc, *opc_out;
        cudaMallocManaged(&opc, ROWS * COLS * 3*sizeof(float));
        cudaMallocManaged(&opc_out, ROWS * COLS * 3*sizeof(float));

    
        // Initialize Memory
        for (int i = 0; i < ROWS; i++)
        {
            for (int j = 0; j < COLS; j++)
            {
                opc[(i * COLS + j)*3 + 0] = i;
                opc[(i * COLS + j)*3 + 1] = j;
                opc[(i * COLS + j)*3 + 2] = 1.0;
                opc_out[(i * COLS + j)*3 + 0] = i;
                opc_out[(i * COLS + j)*3 + 1] = j;
                opc_out[(i * COLS + j)*3 + 2] = 1.0;
            }
        }

        printf("Before Kernel (%.2f)\n", opc[0]);
        dim3 blockShape = dim3( 32, 32 );
        dim3 gridShape  = dim3( (std::ceil((COLS) / (32)), std::ceil((ROWS) / (32))) );
        // Run Kernel
        LaplacianLoopK3<<<blockShape, gridShape>>>(opc, opc_out, ROWS, COLS, lambda);

        cudaDeviceSynchronize();
        printf("Done (%.2f)\n", opc_out[0]);
    

    }

