// simple CQC and graphical wizardry:
//   * auto-exposure
//   * despeckling
//   * 32->24 bpp conversions
//   * Floyd dithering
//
// by: Patrick Reynolds <patrickr@virginia.edu>
// dithering contributed by:
//   Andre Jsemanowicz <andre@andrix.biophysics.mcw.edu>

#include <stdio.h>
#include "config.h"
#include <string.h>
#include <math.h>

// original credits for write_ppm() to the qcam authors,
// modified by Patrick R.
void write_ppm(FILE *output, unsigned char *buf, int width, int height) {
  long i;

  fprintf(output, "P6\n%d %d 255\n", width, height);

  for (i = 0; i < width * height * 3; i++) {
    fputc(buf[i], output);
  }
}

int get_brightness_adj(unsigned char *image, long size, int &brightness) {
  long i, tot = 0;
  for (i=0;i<size*3;i++)
    tot += image[i];
  brightness = (128 - tot/(size*3))/2;
  return ((tot/(size*3)) >= 126 && (tot/(size*3)) <= 130);
}

void get_rgb_adj(unsigned char *image, long size, int &red, int &green,
  int &blue) {
  long i, rtot = 0, gtot = 0, btot = 0;
  for (i=0; i<size; i++) {
    rtot += image[i*3];
    gtot += image[i*3+1];
    btot += image[i*3+2];
  }
  red = 16384 / (rtot/(size));
  green = 16384 / (gtot/(size));
  blue = 16384 / (btot/(size));   
}

#ifndef min
#define min(x, y) (((x) > (y))  ?  (y)  :  (x))
#endif
#ifndef max
#define max(x, y) (((x) > (y))  ?  (x)  :  (y))
#endif

void do_rgb_adj(unsigned char *image, long size, int red, int green,
  int blue) {
  long i;
  for (i=0; i<size; i++) {  
    image[i*3] = min(image[i*3] * red >> 7, 255);
    image[i*3+1] = min(image[i*3+1] * green >> 7, 255);
    image[i*3+2] = min(image[i*3+2] * blue >> 7, 255);
  }  
}

void allocate_rgb_palette(int size, int pal[][3], int rgb[][3]) {
  int i, j, k = 0, m;
  float f0 = 1.0/(size - 1);
  float a;

  int *fix = new int[size];

  for (i=0; i<size; i++) {
    a = i * f0;
    k = (int)(255 * a + 0.5);
    fix[i] = max(0, min(k, 255));
  }

  m = 0;
  for (i=0; i<size; i++)
    for (j=0; j<size; j++)
      for (k=0; k<size; k++) {
        pal[m][0] = fix[i];
        pal[m][1] = fix[j];
        pal[m++][2] = fix[k];
      }
  m = 0;
  for (i=0; i<256; i++) {
    rgb[i][0] = m * size * size;
    rgb[i][1] = m * size;
    rgb[i][2] = m;
    if (i > fix[m] + k)
      k = (fix[m+1] - fix[m++])/2;
  }
  delete[] fix;
}

unsigned char *rgb_2_pal(unsigned char *image, int width, int height,
  int size, int pal[][3], int rgb[][3]) {
  int i;
  int m = 0;
  unsigned char *ret = new unsigned char[width * height];
  for (i=0; i<height; i++) {
    for (int j=0; j<width; j++) {
      int k = rgb[image[m]][0] + rgb[image[m+1]][1] + rgb[image[m+2]][2];
      ret[i * width + j] = max(0, min(k, size*size*size-1));

      int temprgb[3];
      temprgb[0] = image[m] - pal[k][0];
      temprgb[1] = image[m+1] - pal[k][1];
      temprgb[2] = image[m+2] - pal[k][2];

      if (j < width - 1)
        for (int n=0; n<3; n++)
          image[m + 3 + n] =
            max(0, min(255, image[m + 3 + n] + ((temprgb[n] * 7) >> 4)));

      if (i < height - 1) {
        int n;
        if (j > 0)
          for (n=0; n<3; n++)
            image[m + 3*width - 3 + n] =
              max(0, min(255, image[m + 3*width - 3 + n] +
              ((temprgb[n] * 3) >> 4)));
        for (n=0; n<3; n++)
          image[m + 3*width + n] = max(0, min(255, image[m + 3*width + n] +
            ((temprgb[n] * 5) >> 4)));
        if (j < height - 1)
          for (n=0; n<3; n++)
            image[m + 3*width + 3 + n] =
              max(0, min(255, image[m + 3*width + 3 + n] + (temprgb[n] >> 4)));
      } // if
      m += 3;
    } // for j
  } // for i
  delete[] image;
  return ret;
}

unsigned char *despeckle32(unsigned char *image, int width, int height);

unsigned char *raw32_to_24(unsigned char *buf, int width, int height,
  int nospecks) {

// input buffer is of the form BGGR BGGR BGGR ...

  unsigned char *retbuf;
  int i, j;
  if (nospecks)
    buf = despeckle32(buf, width, height/4);
  retbuf = new unsigned char[width * height * 3];

// there's a band on the right and the bottom where the overlapped-
// pixels algorithm fails to provide pixels.  (fix me?)  Blank it out with 
// bzero() so that it will be black.
  bzero(retbuf, width*height*3);
  unsigned char *temp[4];
  for (i=0; i<width; i+=2)
    for (j=0; j<height; j+=2) {

// the **temp array points to the four pixels used in the overlapping scheme
// for details, see the appendix of the ColorQC specs from Connectix
      temp[0] = &buf[i*2 + j*width];
      temp[1] = &buf[i*2+4 + j*width];
      temp[2] = &buf[i*2 + (j+2)*width];
      temp[3] = &buf[i*2+4 + (j+2)*width];

      retbuf[3*i + 3*j*width] = temp[0][3];                // BG // red
      retbuf[2 + 3*i + 3*j*width] = temp[0][0];            // GR // blue
      retbuf[1 + 3*i + 3*j*width] =
        (temp[0][1] + temp[0][2])/2;                             // green

      retbuf[3*(i+1) + 3*j*width] = temp[0][3];            // GB // red
      retbuf[2 + 3*(i+1) + 3*j*width] = temp[1][0];        // RG // blue
      retbuf[1 + 3*(i+1) + 3*j*width] =
        (temp[0][1] + temp[1][2])/2;                             // green

      retbuf[3*i + 3*(j+1)*width] = temp[0][3];            // GR // red
      retbuf[2 + 3*i + 3*(j+1)*width] = temp[2][0];        // BG // blue
      retbuf[1 + 3*i + 3*(j+1)*width] =
        (temp[0][2] + temp[2][1])/2;                             // green

      retbuf[3*(i+1) + 3*(j+1)*width] = temp[0][3];        // RG // red
      retbuf[2 + 3*(i+1) + 3*(j+1)*width] = temp[3][0];    // GB // blue
      retbuf[1 + 3*(i+1) + 3*(j+1)*width] =
        (temp[1][2] + temp[2][1])/2;                             // green
    }
  delete[] buf;

  return retbuf;
}

// the light-check threshold.  Higher numbers remove more lights but blur the
// image more.  30 is good for indoor lighting.
#define NO_LIGHTS 30

// macros to make the code a little more readable, p=previous, n=next
#define RED image[i*3]
#define GREEN image[i*3+1]
#define BLUE image[i*3+2]
#define pRED image[i*3-3]
#define pGREEN image[i*3-2]
#define pBLUE image[i*3-1] 
#define nRED image[i*3+3]
#define nGREEN image[i*3+4] 
#define nBLUE image[i*3+5]

unsigned char *despeckle(unsigned char *image, int width, int height) {
  unsigned char *newimage = new unsigned char[width*height*3];
  if (newimage == NULL) {
    fprintf(stderr, "malloc() failed while allocating %d bytes\n",
      width*height*3);
    exit(1);
  }
  long i;
  for (i=0; i<width*height; i++) {
    if (i % width == 0 || i % width == width - 1)
      memcpy(&newimage[i*3], &image[i*3], 3);
    else {
      if (RED - (GREEN+BLUE)/2 >
        NO_LIGHTS + ((pRED - (pGREEN+pBLUE)/2) +
        (nRED - (nGREEN+nBLUE)/2)))
        newimage[i*3] = (pRED+nRED)/2;
        else newimage[i*3] = RED;
      if (GREEN - (RED+BLUE)/2 >
        NO_LIGHTS + ((pGREEN - (pRED+pBLUE)/2) +
        (nGREEN - (nRED+nBLUE)/2)))
        newimage[i*3+1] = (pGREEN+nGREEN)/2;
        else newimage[i*3+1] = GREEN;
      if (BLUE - (GREEN+RED)/2 >
        NO_LIGHTS + ((pBLUE - (pGREEN+pRED)/2) +
        (nBLUE - (nGREEN+nRED)/2)))
        newimage[i*3+2] = (pBLUE+nBLUE)/2;
        else newimage[i*3+2] = BLUE;
    }  // if width
  }    // for
  delete[] image;
  return newimage;
}

// more macros (undef the old ones first) to make the code more readable
#undef RED
#undef GREEN
#undef BLUE
#undef pRED
#undef pGREEN
#undef pBLUE
#undef nRED
#undef nGREEN
#undef nBLUE

#define RED image[i*4]
#define GREENa image[i*4+1]
#define GREENb image[i*4+2]
#define BLUE image[i*4+3]
#define pRED image[i*4-4]
#define pGREENa image[i*4-3]
#define pGREENb image[i*4-2]
#define pBLUE image[i*4-1]
#define nRED image[i*4+4]
#define nGREENa image[i*4+5]
#define nGREENb image[i*4+6]
#define nBLUE image[i*4+7]

unsigned char *despeckle32(unsigned char *image, int width, int height) {
  unsigned char *newimage = new unsigned char[width*height*4];
  if (newimage == NULL) {
    fprintf(stderr, "malloc() failed while allocating %d bytes\n",
      width*height*4);
    exit(1);
  }
  long i;
  for (i=0; i<width*height; i++) {
    if (i % width == 0 || i % width == width - 1)
      memcpy(&newimage[i*4], &image[i*4], 4);
    else {
      if (RED - ((GREENa+GREENb)/2+BLUE)/2 >
        NO_LIGHTS + ((pRED - ((pGREENa+pGREENb)/2+pBLUE)/2) +
        (nRED - ((nGREENa+nGREENb)/2+nBLUE)/2)))
        newimage[i*4] = (pRED+nRED)/2;
        else newimage[i*4] = RED;
      if (GREENa - (RED+BLUE)/2 >
        NO_LIGHTS + ((pGREENa - (pRED+pBLUE)/2) +
        (nGREENa - (nRED+nBLUE)/2)))
        newimage[i*4+1] = (pGREENa+nGREENa)/2;
        else newimage[i*4+1] = GREENa;
      if (GREENb - (RED+BLUE)/2 >
        NO_LIGHTS + ((pGREENb - (pRED+pBLUE)/2) +
        (nGREENb - (nRED+nBLUE)/2)))
        newimage[i*4+2] = (pGREENb+nGREENb)/2;
        else newimage[i*4+2] = GREENb;
      if (BLUE - ((GREENa+GREENb)/2+RED)/2 >
        NO_LIGHTS + ((pBLUE - ((pGREENa+pGREENb)/2+pRED)/2) +
        (nBLUE - ((nGREENa+nGREENb)/2+nRED)/2)))
        newimage[i*4+3] = (pBLUE+nBLUE)/2;
        else newimage[i*4+3] = BLUE;
    }  // if width
  }    // for
  delete[] image;
  return newimage;
}