#include <stdint.h>

/*
 The function converts a 16bit GRBG 2x2 CFA coded image into an 8bit
 RGB image by setting the missing RGB values to zero.
 */
void debayer_grbg16_simple (uint16_t * src, int src_w, int src_h,
			    uint8_t  * dst, int dst_w, int dst_h) {
  
  // GG RR
  // BB GG
  uint16_t t;
  uint8_t r, g, b;
  int ys, yd, xs, xd;

  for (ys = 0, yd = 0; ys < src_h && yd < dst_h; ys++, yd++) {
    for (xs = 0, xd = 0; xs < src_w; xs++) {

      /* read the pixel but only the higher 8bit, assuming data is little endian */
      t = (*(src++) >> 8) & 0xff;
      
      if (xs & 1) {
	if (ys & 1) {
	  // lower right green pixel
	  b = 0; g = t; r = 0;
	} else {
	  // upper right red pixel
	  b = 0; g = 0; r = t;
	}
      } else {
	if (ys & 1) {
	  // lower left blue pixel
	  b = t; g = 0; r = 0;
	} else {
	  // upper left green pixel
	  b = 0; g = t; r = 0;
	}
      }
      
      /* only paint the image if the source is within the destination */
      if (xd < dst_w) {
	/* set the pixel */
	*(dst++) = r;
	*(dst++) = g;
	*(dst++) = b;
	xd++;
      }
    }
    
    /* if the source image is too small ignore the other places */
    if (xd < dst_w)
      dst += 3 * (dst_w - xd);
  }
}

// macros to make code better readable
#define CE (*(src))
#define UP (*(src-src_w))
#define DN (*(src+src_w))
#define LE (*(src-1))
#define RI (*(src+1))
#define UPLE (*(src-src_w-1))
#define UPRI (*(src-src_w+1))
#define DNLE (*(src+src_w-1))
#define DNRI (*(src+src_w+1))
#define STORE *(dst++) = (r>>8)&0xff; *(dst++) = (g>>8)&0xff; *(dst++) = (b>>8) & 0xff; src++;


/*
 The function converts a 16bit GRBG 2x2 CFA coded image into an 8bit
 RGB image by bilinear interpolation.
 */
void debayer_grbg16_bilinear (uint16_t * src, int src_w, int src_h,
			      uint8_t  * dst, int dst_w, int dst_h) {
  
  // GG RR GG RR
  // BB GG BB GG
  // GG RR GG RR
  // BB GG BB GG
  uint32_t r, g, b;
  int xs, ys;

  // start with upper left pixel (green)
  r = RI;
  g = CE;
  b = DN;
  STORE;

  // upper first line, starts with RR GG RR ...
  for (xs = 1; xs < src_w - 1; xs+=2) {
    // red pixel
    r = CE;
    g = (LE + RI + DN) / 3;
    b = (DNLE + DNRI) / 2;
    STORE;
    // green pixel
    r = (LE + RI) / 2;
    g = CE;
    b = DN;
    STORE;
  }

  // upper right pixel (red)
  r = CE;
  g = (DN + LE) / 2;
  b = DNLE;
  STORE;

  // go through the "body" of the image
  for (ys = 1; ys < src_h - 1; ys+=2) {

    // every second line with BB GG BB GG (start at 2nd line)

    // left hand pixel (blue)
    r = (UPRI + DNRI) / 2;
    g = (UP + DN + RI) / 3;
    b = CE;
    STORE;
    for (xs = 1; xs < src_w - 1; xs+=2) {
      // green pixel
      r = (UP + DN) / 2;
      g = CE;
      b = (LE + RI) / 2;
      STORE;
      // blue pixel
      r = (UPLE + UPRI + DNLE + DNRI) / 4;
      g = (LE + RI + UP + DN) / 4;
      b = CE;
      STORE;
    }
    // last pixel in line (green)
    r = (UP + DN) / 2;
    g = CE;
    b = LE;
    STORE;

    // every second line with GG RR GG RR ... (start at 3rd line)

    // left hand pixel (green)
    r = RI;
    g = CE;
    b = (UP + DN) / 2;
    STORE;
    for (xs = 1; xs < src_w - 1; xs+=2) {
      // red pixel
      r = CE;
      g = (LE + RI + UP + DN) / 4;
      b = (UPLE + UPRI + DNLE + DNRI) / 4;
      STORE;
      // green pixel
      r = (LE + RI) / 2;
      g = CE;
      b = (UP + DN) / 2;
      STORE;
    }
    // last pixel in line (red)
    r = CE;
    g = (UP + DN + LE) / 3;
    b = (UPLE + DNLE) / 2;
    STORE;
  }

  // bottom left pixel
  r = UPRI;
  g = (UP + RI) / 2;
  b = CE;
  STORE;

  // last line starting with GG BB GG ...
  for (xs = 1; xs < src_w - 1; xs+=2) {
    // green pixel
    r = UP;
    g = CE;
    b = (LE + RI) / 2;
    STORE;
    // blue pixel
    r = (UPLE + UPRI) / 2;
    g = (LE + UP + RI) / 2;
    b = CE;
    STORE;
  }
  
  // bottom right pixel (green)
  r = UP;
  g = CE;
  b = LE;
  STORE;
}