__kernel void probe (float * img,  
                        float * h0,
                        float * h1,
                        float * h2,
                        float * h3,
                        float * posValues,  
                        float * out,
                        float16  transformMatrix, 
                        int ySize, 
                        int zSize, 
                        int xSize) 
{  
     int location = get_global_id(0); 
     float4 pos = (float4)(posValues[location *3],
                           posValues[(location *3) + 1], 
                           posValues[(location *3) + 2],1.0f); 
                           
        float probedVal;
        float4 f, imgPt,nf,v, hx, hy, hz, h_tx, h_ty, hMult;
        int4 n1; 
        
        float4 d = (float4) (h0[0],h1[0],h2[0],h3[0]); 
        float4 c = (float4) (h0[1],h1[1],h2[1],h3[1]); 
        float4 b = (float4) (h0[2],h1[2],h2[2],h3[2]); 
        float4 a = (float4) (h0[3],h1[3],h2[3],h3[3]);      
        
        // Transform the value to image space position.  
       imgPt = (float4) (dot(pos,transformMatrix.s0123),
                      dot(pos,transformMatrix.s4567),
                      dot(pos,transformMatrix.s89ab),
                      dot(pos,transformMatrix.scdef));
    
    
        nf = floor(imgPt);
        f = imgPt - nf;
        n1 = convert_int4(nf);
    
        /* //Debugging 
        if(location == 1) 
        {
            printf("\n============\nImage Point: (%f, %f, %f)\n",imgPt.x,imgPt.y,imgPt.z); 
            printf("N: (%d,%d,%d,%d)\n",n1.x,n1.y,n1.z,n1.w); 
            printf("f: (%f,%f,%f,%f)\n",f.x,f.y,f.z,f.w); 
        }
        */ 
    
        // the t value for h(fx - i) 
        float4 t_i = (float4) (f.x + 1.0, f.x, f.x - 1.0, f.x - 2.0);  
        
        // the t value for h(fy - j) 
        float4 t_j = (float4) (f.y + 1.0, f.y, f.y - 1.0, f.y - 2.0);  
        
        float4 t_k = (float4) (f.z + 1.0, f.z, f.z - 1.0, f.z - 2.0);
        float4 h_k = d + t_k * (c + t_k * (b + t_k * a));
	float tx[4], ty[4];
	for (int i = -1;  i <= 2;  i++) {
		for (int j = -1;  j <= 2;  j++) {
		    // compute z axis using vectors
			int index = (n1.x+i) * ySize * zSize + (n1.y+j) * zSize + n1.z-1;
			v = (float4)(
				img[index],
				img[index+1],
				img[index+2],
				img[index+3]);
			ty[j+1] = dot(v, h_k);
		}
		tx[i+1] = dot((float4)(ty[0], ty[1], ty[2], ty[3]), d + t_j * (c + t_j * (b + t_j * a)));
	}
	probedVal = dot((float4)(tx[0],tx[1],tx[2],tx[3]), d + t_i * (c + t_i * (b + t_i * a)));
        
        out[location] = probedVal; 
}