Implement bokeh blur with C++ - c++

Question

We would like to perform bokeh blur on a image. I have tried to test some code below but could not get Circle of Confusion on bright point.
void bokeh(unsigned char *Input, unsigned char *Output, int Width, int Height, int Stride, int Radius)
{
int Channels = Stride / Width;
int rsq = fmax(1, sqrtf(Radius));
for (int y = 0; y < Height; y++)
{
unsigned char * LinePD = Output + y*Stride;
for (int x = 0; x < Width; x++)
{
unsigned int sum[3] = { 0 };
unsigned int weightsum = 0;
for (int ny = std::max(0, y - Radius); ny < std::min(y + Radius, Height); ny++)
{
const unsigned char * sampleLine = Input + ny*Stride;
for (int nx = std::max(0, x - Radius); nx < std::min(x + Radius, Width); nx++)
{
if (sqrtf(nx - x) + sqrtf(ny - y) < rsq)
{
const unsigned char * sample = sampleLine + nx*Channels;
const unsigned char&R = sample[0];
const unsigned char&G = sample[1];
const unsigned char&B = sample[2];
float weight = sqrtf((unsigned char)((21627 * R + 21627 * G + 21627 * B) >> 16));
for (int c = 0; c < Channels; c++)
{
sum[c] += weight*sample[c];
}
weightsum += weight;
}
}
}
for (int c = 0; c < Channels; c++)
{
LinePD[c] = ClampToByte(sum[c] / weightsum);
}
LinePD += Channels;
}
}
}
The source image is:
The result is:
while I expect effect is which like circular in pictures below
seems that I replace sqrtf(nx - x) + sqrtf(ny - y) < rsq
with
powf(nx - x, 2.0) + powf(ny - y, 2.0) < powf(Radius, 2)
and replace float weight = sqrtf((unsigned char)((21627 * R + 21627 * G + 21627 * B) >> 16));
with
float weight = (R + G + B)*1.0f/3.0f;
I could get bokeh blur effect, so how to set the weight to by brightness?