int  yuv_to_mat(unsigned char *data, Mat image)
{
    unsigned char *srcYuv = data;
    int indexY, indexU, indexV;
    unsigned char Y, U, V; 
    unsigned char R, G, B; 
    int WIDTH = image.cols;
    int HEIGHT = image.rows;
    int u_pos = WIDTH * HEIGHT;
    int v_pos = u_pos + u_pos/4;

    for( int i = 0; i < HEIGHT; i++ )
    {
        for (int j = 0; j < WIDTH; j++ )
        {
            Y = srcYuv[j + i * WIDTH];
            indexV = i / 2 * WIDTH/2 + j/2;
            indexU = i / 2 * WIDTH/2 + j/2;
            U = srcYuv[u_pos + indexU];
            V = srcYuv[v_pos + indexV];
            
            // YUV to RGB
            R = Y + 1.403 *(V-128);
            G = Y - 0.34414* (U-128) - 0.71414* (V-128);
            B = Y + 1.772 *(U-128);

            image.at<Vec3b>(i, j)[0] = B;
            image.at<Vec3b>(i, j)[1] = G;
            image.at<Vec3b>(i, j)[2] = R;
        }
    } 

    return 0;
}