  *  @param[in] g_y All \f$P_{m,n}(Y)\f$
  *  @param[in] g_v All \f$P^T_{m,n}(V)\f$
  */
-  __global__
-void algSAT_stage4(const int c_width, const int c_height, float *g_out,
-    const float *g_in,
-    const float *g_y,
-    const float *g_v )
-{
-  const int tx = threadIdx.x, ty = threadIdx.y, bx = blockIdx.x, by = blockIdx.y, col = bx*WS+tx, row0 = by*WS;
-
-  __shared__ float s_block[ WS ][ WS+1 ];
-
-  float (*bdata)[WS+1] = (float (*)[WS+1]) &s_block[ty][tx];
-
-  g_in += (row0+ty)*c_width+col;
-  if( by > 0 ) g_y += (by-1)*c_width+col;
-  if( bx > 0 ) g_v += (bx-1)*c_height+row0+tx;
-
-#pragma unroll
-  for (int i = 0; i < WS-(WS%SOW); i+=SOW) {
+__global__ void algSAT_stage4(const int c_width, const int c_height,
+                              float *g_out, const float *g_in, const float *g_y,
+                              const float *g_v)
+{
+  const int tx = threadIdx.x, ty = threadIdx.y, bx = blockIdx.x,
+            by = blockIdx.y, col = bx * WS + tx, row0 = by * WS;
+
+  __shared__ float s_block[WS][WS + 1];
+
+  float(*bdata)[WS + 1] = (float(*)[WS + 1]) & s_block[ty][tx];
+
+  g_in += (row0 + ty) * c_width + col;
+  if (by > 0)
+    g_y += (by - 1) * c_width + col;
+  if (bx > 0)
+    g_v += (bx - 1) * c_height + row0 + tx;
+
+#pragma unroll
+  for (int i = 0; i < WS - (WS % SOW); i += SOW)
+  {
     **bdata = *g_in;
     bdata += SOW;
     g_in += SOW * c_width;
   }
-  if( ty < WS%SOW ) {
+  if (ty < WS % SOW)
+  {
     **bdata = *g_in;
   }
 
   __syncthreads();
 
-  if( ty == 0 ) {
-
-    {   // calculate y -----------------------
-      float (*bdata)[WS+1] = (float (*)[WS+1]) &s_block[0][tx];
+  if (ty == 0)
+  {
+
+    {  // calculate y -----------------------
+      float(*bdata)[WS + 1] = (float(*)[WS + 1]) & s_block[0][tx];
 
       float prev;
-      if( by > 0 ) prev = *g_y;
-      else prev = 0.f;
+      if (by > 0)
+        prev = *g_y;
+      else
+        prev = 0.f;
 
 #pragma unroll
       for (int i = 0; i < WS; ++i, ++bdata)
         **bdata = prev = **bdata + prev;
     }
 
-    {   // calculate x -----------------------
+    {  // calculate x -----------------------
       float *bdata = s_block[tx];
 
       float prev;
-      if( bx > 0 ) prev = *g_v;
-      else prev = 0.f;
+      if (bx > 0)
+        prev = *g_v;
+      else
+        prev = 0.f;
 
 #pragma unroll
       for (int i = 0; i < WS; ++i, ++bdata)
         *bdata = prev = *bdata + prev;
     }
-
   }
 
   __syncthreads();
 
-  bdata = (float (*)[WS+1]) &s_block[ty][tx];
-
-  g_out += (row0+ty)*c_width+col;
-
-#pragma unroll
-  for (int i = 0; i < WS-(WS%SOW); i+=SOW) {
+  bdata = (float(*)[WS + 1]) & s_block[ty][tx];
+
+  g_out += (row0 + ty) * c_width + col;
+
+#pragma unroll
+  for (int i = 0; i < WS - (WS % SOW); i += SOW)
+  {
     *g_out = **bdata;
     bdata += SOW;
     g_out += SOW * c_width;
   }
-  if( ty < WS%SOW ) {
+  if (ty < WS % SOW)
+  {
     *g_out = **bdata;
   }
 }

      *g_ybar = prev;
    }

    {   // calculate vhat -----------------------
      float *bdata = s_block[tx];

      float prev = *bdata;

}
*/

__global__ void sat_init_kernel(int image_size, float xscale, float yscale,
                                float zths, float *thrustptr)
{
  int x = blockIdx.x * blockDim.x + threadIdx.x;
  int y = blockIdx.y * blockDim.y + threadIdx.y;
  int index = y * image_size + x;

  float texval = tex2D(textureReadDepth, x * xscale + 0.5f, y * yscale + 0.5f);

  thrustptr[index] = texval;
}

void cudaSAT(cudaGraphicsResource_t &inputImage, int image_size,
             int screen_size_x, int screen_size_y, float z_threshold,
             thrust::device_vector<float> &inout,
             thrust::device_vector<float> &ybar,
             thrust::device_vector<float> &vhat,
             thrust::device_vector<float> &ysum)
{
  int compute_width = image_size;
  int compute_height = image_size;
  if (compute_width % 32 > 0)
    compute_width += (32 - (compute_width % 32));
  if (compute_height % 32 > 0)
    compute_height += (32 - (compute_height % 32));
  int compute_m_size = (compute_width + WS - 1) / WS;
  int compute_n_size = (compute_height + WS - 1) / WS;

  // set data structure sizes

  if (inout.size() !=
      static_cast<unsigned long>(compute_width * compute_height))
  {
    inout.resize(compute_width * compute_height);
  }
  if (ybar.size() != static_cast<unsigned long>(compute_n_size * compute_width))
  {
    ybar.resize(compute_n_size * compute_width);
  }
  if (vhat.size() !=
      static_cast<unsigned long>(compute_m_size * compute_height))
  {
    vhat.resize(compute_m_size * compute_height);
  }
  if (ysum.size() !=
      static_cast<unsigned long>(compute_m_size * compute_n_size))
  {
    ysum.resize(compute_m_size * compute_n_size);
  }

  float *d_inout = thrust::raw_pointer_cast(inout.data());
  float *d_ybar = thrust::raw_pointer_cast(ybar.data());
  float *d_vhat = thrust::raw_pointer_cast(vhat.data());
  float *d_ysum = thrust::raw_pointer_cast(ysum.data());

  // QElapsedTimer tm;
  // tm.start();

  // initialize occupancy function from inputImage
  textureReadDepth.normalized = 0;
