#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <string>

std::string toBits(unsigned int i, int maxbits);

/*
 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 BITCONV(d) ((d>>8) & 0xff)
#define STORE  *(dst++) = BITCONV(r); *(dst++) = BITCONV(g); *(dst++) = BITCONV(b); src++;
#define STORE8 *(dst++) = (r & 0xff); *(dst++) = (g & 0xff); *(dst++) = (b & 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;
}

/*
 The function converts a 8bit GRBG 2x2 CFA coded image into an 8bit
 RGB image by setting the missing RGB values to zero.
 */
void debayer_grbg8_simple (uint8_t * src, int src_w, int src_h,
			   uint8_t * dst, int dst_w, int dst_h) {
  
  // GG RR
  // BB GG
  uint8_t t;
  uint16_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++);
      
      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);
  }
}

/*
 The function converts a 8bit GRBG 2x2 CFA coded image into an 8bit
 RGB image by bilinear interpolation.
 */
void debayer_grbg8_bilinear (uint8_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
  uint16_t r, g, b;
  int xs, ys;

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

  // 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;
    STORE8;
    // green pixel
    r = (LE + RI) / 2;
    g = CE;
    b = DN;
    STORE8;
  }

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

  // 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;
    STORE8;
    for (xs = 1; xs < src_w - 1; xs+=2) {
      // green pixel
      r = (UP + DN) / 2;
      g = CE;
      b = (LE + RI) / 2;
      STORE8;
      // blue pixel
      r = (UPLE + UPRI + DNLE + DNRI) / 4;
      g = (LE + RI + UP + DN) / 4;
      b = CE;
      STORE8;
    }
    // last pixel in line (green)
    r = (UP + DN) / 2;
    g = CE;
    b = LE;
    STORE8;

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

    // left hand pixel (green)
    r = RI;
    g = CE;
    b = (UP + DN) / 2;
    STORE8;
    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;
      STORE8;
      // green pixel
      r = (LE + RI) / 2;
      g = CE;
      b = (UP + DN) / 2;
      STORE8;
    }
    // last pixel in line (red)
    r = CE;
    g = (UP + DN + LE) / 3;
    b = (UPLE + DNLE) / 2;
    STORE8;
  }

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

  // 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;
    STORE8;
    // blue pixel
    r = (UPLE + UPRI) / 2;
    g = (LE + UP + RI) / 2;
    b = CE;
    STORE8;
  }
  
  // bottom right pixel (green)
  r = UP;
  g = CE;
  b = LE;
  STORE8;
}

int MAX(int a, int m) { if (a > m) return m;	else return a; };
#define PRGGB10P_U ((uint16_t)(*(psrc-src_w*2*10/8)))
#define PRGGB10P_D ((uint16_t)(*(psrc+src_w*2*10/8)))
#define PRGGB10P_L ((uint16_t)(*(psrc-2)*10/8))
#define PRGGB10P_R ((uint16_t)(*(psrc+2)*10/8))
#define PRGGB10P_UL ((uint16_t)(*(psrc-2-src_w*2*10/8)))
#define PRGGB10P_UR ((uint16_t)(*(psrc+2-src_w*2*10/8)))
#define PRGGB10P_DL ((uint16_t)(*(psrc-2-src_w*2*10/8)))
#define PRGGB10P_DR ((uint16_t)(*(psrc+2+src_w*2*10/8)))
void debayer_rggb10packet_simple (uint8_t * src, int src_w, int src_h,
                              uint8_t  * dst, int dst_w, int dst_h) {
	int s, d;
	int xs, ys, xd, yd;
	unsigned char r, g, b;
	int max = dst_w * dst_h * 3;
	unsigned char *pdst;
	unsigned char *psrc;
	int shift;

	printf ("%s:%d src size:%dx%d  dst size:%dx%d\n", __FILE__, __LINE__, src_w, src_h, dst_w, dst_h);

	// RR GG RR GG HH RR
	// GG BB GG BB HH GG	
	// RR GG RR GG HH RR
	// GG BB GG BB HH GG	


	for (ys = 0, yd = 0, psrc = src, pdst = dst; ys < dst_h; ys++, yd++) {
		r = 0;
		g = 0;
		b = 0;
		for (xs = 0, xd = 0; xd < dst_w; xs++, xd++) {
			r = 0; b = 0; g = 0;
			if (ys > 0 && ys < src_h-1 && xs > 0 && xs < src_w-1) {
				if (ys&1) {	  
					if (xs&1) {
						b = *psrc;
						g = (PRGGB10P_L + PRGGB10P_R + PRGGB10P_U + PRGGB10P_D)/4;
						r = (PRGGB10P_UL + PRGGB10P_DL + PRGGB10P_UR + PRGGB10P_UL)/4;
					}
					else {
						b = (PRGGB10P_L + PRGGB10P_R)/2;
						g = *psrc;
						r = (PRGGB10P_U + PRGGB10P_D)/2;
					}
				}
				else {
					if (xs&1) {
						b = (PRGGB10P_U + PRGGB10P_D)/2;
						g = *psrc;
						r = (PRGGB10P_L + PRGGB10P_R)/2;
					}
					else {
						g = (PRGGB10P_L + PRGGB10P_R + PRGGB10P_U + PRGGB10P_D)/4;
						b = (PRGGB10P_UL + PRGGB10P_DL + PRGGB10P_UR + PRGGB10P_UL)/4;
						r = *psrc;
					}
				}
			}
			if (pdst - dst > max) {
				printf ("debayer error. dpst out of bounds size:%dx%d  pos:%dx%d  \n", dst_w, dst_h, xd, yd);
			}
			if ((ys == 10 || ys == 11) && xs < 10) printf ("%dx%d - %d, %d, %d\n", xs, ys, r, g, b);
			pdst[0] = MAX(r, 0xff);
			pdst[1] = MAX(g, 0xff);
			pdst[2] = MAX(b, 0xff);
			
			if (yd < 100 && xd < 300) {
				pdst[0] = 0; if (xd < 100) pdst[0] = 255;
				pdst[1] = 0; if (xd > 100 && xd < 200) pdst[1] = 255;
				pdst[2] = 0; if (xd > 200 && xd < 300) pdst[2] = 255;
			}
			
			pdst += 3;
			psrc++;
			if (xs && (xs % 4 == 0)) psrc++;
		}
	}
};


std::string toBits(unsigned int i, int maxbits) {
	int b;
	std::string out = "";
	for (b = 0; b < maxbits; b++) {
		if (b % 8 == 0 && b) out = ":" + out;
		if (i % 2) out =  "1" + out;
		else out = "0" + out;
		i = i >> 1;
	}
	return out;
}

void debayer_rggb10packet_bilinear (uint8_t * src, int src_w, int src_h,
                             uint8_t * dst, int dst_w, int dst_h) {

	debayer_rggb10packet_simple (src, src_w, src_h, dst, dst_w, dst_h);

};


inline int swap16(uint16_t i) {
  uint16_t r;
  
  *(((unsigned char *)&r)) = *(((unsigned char *)&i)+1);
  *(((unsigned char *)&r)+1) = *(((unsigned char *)&i));
  return r;
}

#define PRGGB10_U ((uint16_t)(*(psrc-src_w*2)))
#define PRGGB10_D ((uint16_t)(*(psrc+src_w*2)))
#define PRGGB10_L ((uint16_t)(*(psrc-2)))
#define PRGGB10_R ((uint16_t)(*(psrc+2)))
#define PRGGB10_UL ((uint16_t)(*(psrc-2-src_w*2)))
#define PRGGB10_UR ((uint16_t)(*(psrc+2-src_w*2)))
#define PRGGB10_DL ((uint16_t)(*(psrc-2-src_w*2)))
#define PRGGB10_DR ((uint16_t)(*(psrc+2+src_w*2)))
void debayer_rggb10_simple (uint8_t * src, int src_w, int src_h,
                            uint8_t  * dst, int dst_w, int dst_h) {
  
  int xs, ys, xd, yd;
  uint16_t r, g, b, data;
	int max = dst_w * dst_h * 3;
	unsigned char *pdst;
	unsigned char *psrc;

	for (ys = 0, yd = 0, psrc = src, pdst = dst; ys < dst_h; ys++, yd++) {
		r = 0;
		g = 0;
		b = 0;
		for (xs = 0, xd = 0; xd < dst_w; xs++, xd++) {
			r = 0; b = 0; g = 0;
			if (ys > 0 && ys < src_h-1 && xs > 0 && xs < src_w-1) {
				if (ys&1) {	  
					if (xs&1) {
						b = *psrc;
						g = (PRGGB10_L + PRGGB10_R + PRGGB10_U + PRGGB10_D)/4;
						r = (PRGGB10_UL + PRGGB10_DL + PRGGB10_UR + PRGGB10_UL)/4;
					}
					else {
						b = (PRGGB10_L + PRGGB10_R)/2;
						g = *psrc;
						r = (PRGGB10_U + PRGGB10_D)/2;
					}
				}
				else {
					if (xs&1) {
						b = (PRGGB10_U + PRGGB10_D)/2;
						g = *psrc;
						r = (PRGGB10_L + PRGGB10_R)/2;
					}
					else {
						g = (PRGGB10_L + PRGGB10_R + PRGGB10_U + PRGGB10_D)/4;
						b = (PRGGB10_UL + PRGGB10_DL + PRGGB10_UR + PRGGB10_UL)/4;
						r = *psrc;
					}
				}
			}
			if (pdst - dst > max) {
				printf ("debayer error. dpst out of bounds size:%dx%d  pos:%dx%d  \n", dst_w, dst_h, xd, yd);
			}
			if ((ys == 10 || ys == 11) && xs < 10) printf ("%dx%d - %d, %d, %d\n", xs, ys, r, g, b);
			pdst[0] = (r);
			pdst[1] = (g);
			pdst[2] = (b);
			pdst += 3;
			psrc += 2;
		}
	}
};


void debayer_rggb10_bilinear (uint8_t * src, int src_w, int src_h,
                             uint8_t * dst, int dst_w, int dst_h) {
    debayer_rggb10_simple(src, src_w, src_h, dst, dst_w, dst_h);
};