RGB ->YUV420

unsigned char* readBMP(char* filename)
{
    int i;
    FILE* f;
     fopen_s(&f,filename, "rb");
     FILE* createYUV;
         fopen_s(&createYUV, "Image.yuv", "wb");
 
     if (f == NULL)
         throw "Argument Exception";
 
     unsigned char info[54];
     fread(info, sizeof(unsigned char), 54, f); // read the 54-byte header
 
     // extract image height and width from header
     int width = *(int*)&info[18];
     int height = *(int*)&info[22];
 
     std::cout << std::endl;
     std::cout << "  Name: " << filename << std::endl;
     std::cout << " Width: " << width << std::endl;
     std::cout << "Height: " << height << std::endl;
 
     int row_padded = (width * 3 + 3) & (~3);
     unsigned char* data = new unsigned char[row_padded];
     unsigned char tmp;
     double Y, U, V = 0;
     for (int i = 0; i < height; i++)
     {
         fread(data, sizeof(unsigned char), row_padded, f);
         for (int j = 0; j < width * 3; j += 3)
         {
             // Convert (B, G, R) to (R, G, B)
             tmp = data[j];
             data[j] = data[j + 2];
             data[j + 2] = tmp;
 
             double R = (double)data[j];
             double G = (double)data[j + 1];
             double B = (double)data[j + 2];
             YUVfromRGB(Y, U, V, R, G, B);
             fwrite(&Y, sizeof(Y), 1, createYUV);
             fwrite(&U, sizeof(U), 1, createYUV);
             fwrite(&V, sizeof(V), 1, createYUV);
            std::cout << "R: " << (int)data[j] << " G: " << (int)data[j + 1] << " B: " << (int)data[j + 2] << std::endl;
             //std::cout << "R: " << data[j] << " G: " << (int)data[j + 1] << " B: " << data[j + 2] << std::endl;
         }
        
     }
     fclose(createYUV);
     fclose(f);
     return data;
}
void YUVfromRGB(double& Y, double& U, double& V, const double R, const double G, const double B)
{
    Y = 0.299 * R + 0.587 * G + 0.114 * B;
    U = -0.14713 * R - 0.28886 * G + 0.436 * B + 128;
    V = 0.615 * R - 0.51499 * G - 0.10001 * B + 128;
}