//
// Copyright 2002,2003,2004 Sony Corporation 
//
// Permission to use, copy, modify, and redistribute this software for
// non-commercial use is hereby granted.
//
// This software is provided "as is" without warranty of any kind,
// either expressed or implied, including but not limited to the
// implied warranties of fitness for a particular purpose.
//

#include "../const.h"
#include "BMP.h"

#ifdef AIBO_YES

#include <OPENR/OFbkImage.h>
#include <OPENR/OSyslog.h>


bool 
BMP::SaveYCrCb2RGB(char* path,
                   OFbkImageVectorData* imageVec, OFbkImageLayer layer)
{
    if (layer == ofbkimageLAYER_C) return false;

    byte pixels[3];
    OFbkImageInfo* info = imageVec->GetInfo(layer);
    byte*          data = imageVec->GetData(layer);
    
    OFbkImage yImage(info, data, ofbkimageBAND_Y);
    OFbkImage crImage(info, data, ofbkimageBAND_Cr);
    OFbkImage cbImage(info, data, ofbkimageBAND_Cb);

    BMPHeader     header;
    BMPInfoHeader infoheader;

    infoheader.width     = yImage.Width();
    infoheader.height    = yImage.Height();
    infoheader.imagesize = yImage.Width() * yImage.Height() * 3;

    header.size = infoheader.imagesize + 54;


    FILE* fp = fopen(path, "wb");
    if (fp == 0) {
        PRN(("can't open %s", path));
        return false;
    }

    SaveBMPHeader(fp, header);
    SaveBMPInfoHeader(fp, infoheader);

    for (int y = infoheader.height - 1 ; y >= 0; y--) {
        for (int x = 0; x < infoheader.width; x++) {
            YCrCb2RGB(yImage.Pixel(x, y),
                      crImage.Pixel(x, y),
                      cbImage.Pixel(x, y),
                      &pixels[R_PIXEL], &pixels[G_PIXEL], &pixels[B_PIXEL]);
            fwrite(pixels, 1, 3, fp);
        }
    }

    fclose(fp);
    return true;
}

bool 
BMP::SaveLayerC(char* basepath, OFbkImageVectorData* imageVec, int numChan)
{
    byte pixels[3];
    char path[128];
    OFbkImageInfo* info = imageVec->GetInfo(ofbkimageLAYER_C);
    byte*          data = imageVec->GetData(ofbkimageLAYER_C);

    OFbkImage cdtImage(info, data, ofbkimageBAND_CDT);

    BMPHeader     header;
    BMPInfoHeader infoheader;

    infoheader.width     = cdtImage.Width();
    infoheader.height    = cdtImage.Height();
    infoheader.imagesize = cdtImage.Width() * cdtImage.Height() * 3;

    header.size = infoheader.imagesize + 54;

    for (int i = 0; i < numChan; i++) {

        byte plane = 0x01 << i;
        sprintf(path, "%s%d.BMP", basepath, i);

        FILE* fp = fopen(path, "wb");
        if (fp == 0) {
            PRN(("can't open %s", path));
            return false;
        }

        SaveBMPHeader(fp, header);
        SaveBMPInfoHeader(fp, infoheader);

        for (int y = infoheader.height - 1 ; y >= 0; y--) {
            for (int x = 0; x < infoheader.width; x++) {
                if (cdtImage.Pixel(x, y) & plane) {
                    pixels[R_PIXEL] = pixels[G_PIXEL] = pixels[B_PIXEL] = WHITE;
                } else {
                    pixels[R_PIXEL] = pixels[G_PIXEL] = pixels[B_PIXEL] = BLACK;
                }
                fwrite(pixels, 1, 3, fp);
            }
        }

        fclose(fp);
    }

    return true;
}

bool
BMP::SaveRaw2Gray(char* path,
                  byte* image, int width, int height, int skip)
{
    byte pixels[3];
    BMPHeader     header;
    BMPInfoHeader infoheader;

    infoheader.width     = width;
    infoheader.height    = height;
    infoheader.imagesize = width * height * 3;

    header.size = infoheader.imagesize + 54;

    FILE* fp = fopen(path, "wb");
    if (fp == 0) {
        PRN(("can't open %s", path));
        return false;
    }

    SaveBMPHeader(fp, header);
    SaveBMPInfoHeader(fp, infoheader);

    for (int y = infoheader.height - 1 ; y >= 0; y--) {
        for (int x = 0; x < infoheader.width; x++) {
            byte pixel = *(image + (width + skip) * y + x);
            pixels[R_PIXEL] = pixels[G_PIXEL] = pixels[B_PIXEL] = pixel;
            fwrite(pixels, 1, 3, fp);
        }
    }

    fclose(fp);
    return true;
}

// Laura's
bool
BMP::SaveRaw2RGB(char* path,
                 byte* image, int width, int height)
{
    byte pixels[3];
    BMPHeader     header;
    BMPInfoHeader infoheader;

    infoheader.width     = width;
    infoheader.height    = height;
    infoheader.imagesize = width * height * 3;

    header.size = infoheader.imagesize + 54;

    FILE* fp = fopen(path, "wb");
    if (fp == 0) {
        PRN(("can't open %s", path));
        return false;
    }

    SaveBMPHeader(fp, header);
    SaveBMPInfoHeader(fp, infoheader);

    for (int y = infoheader.height - 1 ; y >= 0; y--) {
        for (int x = 0; x < infoheader.width; x++) {
          byte* ptr = image + 3 * (width * y + x);
          YCrCb2RGB( ptr[0]  , ptr[2] , ptr[1], &pixels[R_PIXEL], &pixels[G_PIXEL], &pixels[B_PIXEL]);
          fwrite(pixels, 1, 3, fp);
        }
    }
    
    fclose(fp);
    return true;
}


// New useful methods...

bool
BMP::SaveYBand(char* path, OFbkImageVectorData* imageVec, OFbkImageLayer layer)
{
    
    if (layer == ofbkimageLAYER_C) return false;

    byte pixels[3];
    OFbkImageInfo* info = imageVec->GetInfo(layer);
    byte*          data = imageVec->GetData(layer);

    OFbkImage yImage(info, data, ofbkimageBAND_Y);

    BMPHeader     header;
    BMPInfoHeader infoheader;

    infoheader.width     = yImage.Width();
    infoheader.height    = yImage.Height();
    infoheader.imagesize = yImage.Width() * yImage.Height() * 3;

    header.size = infoheader.imagesize + 54;

    FILE* fp = fopen(path, "wb");
    if (fp == 0) {
        PRN(("can't open %s", path));
        return false;
    }

    SaveBMPHeader(fp, header);
    SaveBMPInfoHeader(fp, infoheader);

    for (int y = infoheader.height - 1 ; y >= 0; y--) {
        for (int x = 0; x < infoheader.width; x++) {
            pixels[R_PIXEL] = pixels[G_PIXEL] = pixels[B_PIXEL] = yImage.Pixel(x, y);
            fwrite(pixels, 1, 3, fp);
        }
    }

    fclose(fp);
    return true;
}

bool
BMP::SaveCbBand(char* path, OFbkImageVectorData* imageVec, OFbkImageLayer layer)
{
    
    if (layer == ofbkimageLAYER_C) return false;

    byte pixels[3];
    OFbkImageInfo* info = imageVec->GetInfo(layer);
    byte*          data = imageVec->GetData(layer);
    
    OFbkImage cbImage(info, data, ofbkimageBAND_Cb);

    BMPHeader     header;
    BMPInfoHeader infoheader;

    infoheader.width     = cbImage.Width();
    infoheader.height    = cbImage.Height();
    infoheader.imagesize = cbImage.Width() * cbImage.Height() * 3;

    header.size = infoheader.imagesize + 54;

    FILE* fp = fopen(path, "wb");
    if (fp == 0) {
        PRN(("can't open %s", path));
        return false;
    }

    SaveBMPHeader(fp, header);
    SaveBMPInfoHeader(fp, infoheader);

    for (int y = infoheader.height - 1 ; y >= 0; y--) {
        for (int x = 0; x < infoheader.width; x++) {
            pixels[R_PIXEL] = pixels[G_PIXEL] = pixels[B_PIXEL] = cbImage.Pixel(x, y);
            fwrite(pixels, 1, 3, fp);
        }
    }

    fclose(fp);
    return true;
}

bool
BMP::SaveCrBand(char* path, OFbkImageVectorData* imageVec, OFbkImageLayer layer)
{
    
    if (layer == ofbkimageLAYER_C) return false;

    byte pixels[3];
    OFbkImageInfo* info = imageVec->GetInfo(layer);
    byte*          data = imageVec->GetData(layer);

    OFbkImage crImage(info, data, ofbkimageBAND_Cr);

    BMPHeader     header;
    BMPInfoHeader infoheader;

    infoheader.width     = crImage.Width();
    infoheader.height    = crImage.Height();
    infoheader.imagesize = crImage.Width() * crImage.Height() * 3;

    header.size = infoheader.imagesize + 54;

    FILE* fp = fopen(path, "wb");
    if (fp == 0) {
        PRN(("can't open %s", path));
        return false;
    }

    SaveBMPHeader(fp, header);
    SaveBMPInfoHeader(fp, infoheader);

    for (int y = infoheader.height - 1 ; y >= 0; y--) {
        for (int x = 0; x < infoheader.width; x++) {
            pixels[R_PIXEL] = pixels[G_PIXEL] = pixels[B_PIXEL] = crImage.Pixel(x, y);
            fwrite(pixels, 1, 3, fp);
        }
    }

    fclose(fp);
    return true;
}

#endif


void
BMP::SaveBMPHeader(FILE* fp, const BMPHeader& header)
{
    fwrite(header.magic, 1, 2, fp);
    write_longword(fp, header.size);
    write_word(fp, header.reserved1);
    write_word(fp, header.reserved2);
    write_longword(fp, header.offset);
}

void
BMP::SaveBMPInfoHeader(FILE* fp, const BMPInfoHeader& infoheader)
{
    fwrite(&infoheader, sizeof(infoheader), 1, fp);
}

void
BMP::YCrCb2RGB(byte y, byte cr, byte cb, byte* r, byte* g, byte* b)
{
    // offset binary [0, 255] -> signed char [-128, 127]
    sbyte scr = (sbyte)(cr ^ 0x80);
    sbyte scb = (sbyte)(cb ^ 0x80);

    double Y  = (double)y / 255.0;   //  0.0 <= Y  <= 1.0
    double Cr = (double)scr / 128.0; // -1.0 <= Cr <  1.0
    double Cb = (double)scb / 128.0; // -1.0 <= Cb <  1.0

    double R = 255.0 * (Y + Cr);
    double G = 255.0 * (Y - 0.51*Cr - 0.19*Cb);
    double B = 255.0 * (Y + Cb);

    if (R > 255.0) {
        *r = 255;
    } else if (R < 0.0) {
        *r = 0;
    } else {
        *r = (byte)R;
    }

    if (G > 255.0) {
        *g = 255;
    } else if (G < 0.0) {
        *g = 0;
    } else {
        *g = (byte)G;
    }

    if (B > 255.0) {
        *b = 255;
    } else if (B < 0.0) {
        *b = 0;
    } else {
        *b = (byte)B;
    }
}

void
BMP::write_word(FILE* fp, word w)
{
    fputc(w & 0xff, fp);
    fputc((w >> 8) & 0xff, fp);
}

void
BMP::write_longword(FILE* fp, longword l)
{
    fputc(l & 0xff, fp);
    fputc((l >> 8) & 0xff, fp);
    fputc((l >> 16) & 0xff, fp);
    fputc((l >> 24) & 0xff, fp);
}

void
BMP::write_slongword(FILE* fp, slongword sl)
{
    fputc(sl & 0xff, fp);
    fputc((sl >> 8) & 0xff, fp);
    fputc((sl >> 16) & 0xff, fp);
    fputc((sl >> 24) & 0xff, fp);
}




// This function prints an Image to a file in BMP format
bool
BMP::Write2File(char* path, const Image& image)  {

#ifdef FUNC_TRACK
PRN(("Starting Write2File\n"));
#endif

byte pixels[3];
byte padding = 0;
int x, y, r;
fpos_t pos;

BMPHeader     header;
BMPInfoHeader infoheader;

// each row must have a muliple-of-four number of bytes!
r = (image.width * 3) % 4;

infoheader.width =  image.width;
infoheader.height = image.height;

if (!r)
   infoheader.imagesize = infoheader.width * infoheader.height * 3;
else
   infoheader.imagesize = (infoheader.width * infoheader.height * 3) + (4 - r) * infoheader.height;
header.size = infoheader.imagesize + 54;


FILE* f = fopen(path, "wb+");
if (f == 0) {
        PRN(("can't open %s", path));
        return false;
}

SaveBMPHeader(f, header);
SaveBMPInfoHeader(f, infoheader);


// Writing file
pos = header.offset; // = 54
fsetpos(f, &pos);
for (y = infoheader.height - 1 ; y >= 0; y--) {
        for (x = 0; x < infoheader.width; x++) {
            pixels[R_PIXEL] = pixels[G_PIXEL] = pixels[B_PIXEL] = image.data[x][y];
            fwrite(pixels, 1, 3, f);
        }
        // Padding
        if (r) {
           for (x = 0; x < 4 - r; x++) {
               fwrite(&padding, 1, 1, f);
           }
        }
}


fclose(f);

#ifdef FUNC_TRACK
PRN(("Ending Write2File\n"));
#endif

return true;

}

// This function prints an Image to a file in BMP format, highlighting the given ('show') component
bool
BMP::WriteAndHighlight(char* path, const Image& image, const Image& show)  {

#ifdef FUNC_TRACK
PRN(("Starting WriteAndHighlight\n"));
#endif

byte pixels[3];
byte padding = 0;
int x, y, r;
fpos_t pos;

BMPHeader     header;
BMPInfoHeader infoheader;

// each row must have a multiple-of-four number of bytes!
r = (image.width * 3) % 4;

infoheader.width =  image.width;
infoheader.height = image.height;

if (!r)
   infoheader.imagesize = infoheader.width * infoheader.height * 3;
else
   infoheader.imagesize = (infoheader.width * infoheader.height * 3) + (4 - r) * infoheader.height;
header.size = infoheader.imagesize + 54;


FILE* f = fopen(path, "wb+");
if (f == 0) {
        PRN(("can't open %s", path));
        return false;
}

SaveBMPHeader(f, header);
SaveBMPInfoHeader(f, infoheader);

// Writing file
pos = header.offset; // = 54
fsetpos(f, &pos);
for (y = infoheader.height - 1 ; y >= 0; y--) {
    for (x = 0; x < infoheader.width; x++) {
        if (show.data[x][y]) {
              pixels[R_PIXEL] = WHITE;
              pixels[G_PIXEL] = pixels[B_PIXEL] = BLACK;
        }
        else
              pixels[R_PIXEL] = pixels[G_PIXEL] = pixels[B_PIXEL] = image.data[x][y];
        fwrite(pixels, 1, 3, f);
     }
     // Padding
     if (r) {
         for (x = 0; x < 4 - r; x++) {
             fwrite(&padding, 1, 1, f);
         }
     }
}

fclose(f);

#ifdef FUNC_TRACK
PRN(("Ending WriteAndHighlight\n"));
#endif

return true;

}





