   uint32_t idx = blockDim.x*blockIdx.x + threadIdx.x;
 
   // Check that we are in range
-  if(idx >= size_vals)
+  if(idx >= size_total)
     return;
 
   // Else get our value
-  uint32_t u = u_vals[idx];
-  uint32_t v = v_vals[idx];
+  int32_t id_tri = disparity_grid[idx];
+
+
+  uint32_t u = u_vals[id_tri][idx];
+  uint32_t v = v_vals[id_tri][idx];
 
   // address of disparity we want to compute
   uint32_t d_addr;

          int32_t v_2 = (uint32_t)(BC_a*(float)u+BC_b);
          for (int32_t v=min(v_1,v_2); v<max(v_1,v_2); v++)
            if (!param.subsampling || v%2==0) {
              to_calc.push_back(std::pair<int32_t,int32_t>(u,v));
            }
        }
      }
    }

    int size = to_calc.size()*sizeof(int32_t);
    // Convert vector to array
    int32_t* u_vals = (int32_t*)malloc(size);
    int32_t* v_vals = (int32_t*)malloc(size);

    // Save to arrays
    for(size_t j=0; j < to_calc.size(); j++) {
      u_vals[j] = to_calc.at(j).first;
      v_vals[j] = to_calc.at(j).second;
    }

    // Copy to device code
    int32_t* d_u_vals, *d_v_vals;
    cudaMalloc((void**) &d_u_vals, size);
    cudaMalloc((void**) &d_v_vals, size);
    cudaMemcpy(d_u_vals, u_vals, size, cudaMemcpyHostToDevice);
    cudaMemcpy(d_v_vals, v_vals, size, cudaMemcpyHostToDevice);

    // Calculate size of kernel
    int block_size = 32;
    int grid_size = 0;
    //Calculate gridsize (Add 1 if not evenly divided)
    if(to_calc.size()%block_size == 0){
        grid_size = ceil(to_calc.size()/block_size);
    }else{
        grid_size = ceil(to_calc.size()/block_size) + 1;
    }

    dim3 DimGrid(grid_size,1,1);
    dim3 DimBlock(block_size,1,1);

    // cout << "Cuda Elem Size: " << to_calc.size() << endl;
    // cout << "Cuda Block Size: " << block_size << endl;
    // cout << "Cuda Grid Size: " << grid_size << endl;

    // Next launch our CUDA kernel
    // TODO: Convert this to CUDA kernel
    // for(size_t j=0; j < to_calc.size(); j++) {
    //   int u = to_calc.at(j).first;
    //   int v = to_calc.at(j).second;
    //   // CPU Method
    //   findMatch(u,v,plane_a,plane_b,plane_c,disparity_grid,grid_dims,I1_desc,I2_desc,P,plane_radius,valid,right_image,D);
    // }

    //GPU Method
    findMatch_GPU<<<DimGrid, DimBlock>>>(d_u_vals,d_v_vals,to_calc.size(),plane_a,plane_b,plane_c,d_disparity_grid,d_grid_dims,
                                        d_I1,d_I2,d_P,plane_radius,width,height,valid,right_image,d_D);
    
    cudaDeviceSynchronize();
    cudaFree(d_u_vals);
    cudaFree(d_v_vals);
    
  }

  // Copy the final disparity values back over
  
  cudaMemcpy(disparity_grid, d_disparity_grid, grid_dims[0]*grid_dims[1]*grid_dims[2]*sizeof(int32_t), cudaMemcpyDeviceToHost);
  cudaMemcpy(D, d_D, width*height*sizeof(float), cudaMemcpyDeviceToHost);

  
  // Free local memory
  delete[] P;


  // Free cuda memory
  cudaFree(d_disparity_grid);

  int32_t c1, c2, c3;
  float plane_a,plane_b,plane_c,plane_d;

  // Size variables
  int32_t size_total = 0;
  int32_t size_tri = tri.size();

  // Master objects that will need to be created
  // These will be passed to the CUDA kernel
  float* planes = new float[4*size_tri];
  int32_t* size_uv = new int32_t[size_tri];
  int32_t** vals_u = new *int32_t[size_tri];
  int32_t** vals_v = new *int32_t[size_tri];


  // for all triangles do
  for (uint32_t i=0; i<tri.size(); i++) {
