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

#include <windows.h>

typedef struct {
	int width;
	int height;
	int threshold;
} OPTION;

static int parse_args(OPTION *dst, int argc, char **argv);
static int parse_int_option(int *dst, int idx, int argc, char **argv);
static void show_usage(const char *cmd);
static void copy_basename(char *dst, const char *src);
static void adaptive_filter(const char *src_path, const char *dst_path, const OPTION *opt);
static void adaptive_5x5(unsigned char *dst, const unsigned char *src, int width, int height, int margin, int threshold);
static void adaptive_5x5_simd(unsigned char *dst, const unsigned char *src, int width, int height, int margin, int threshold);

int main(int argc, char **argv)
{
	OPTION opt;
	int i;

	i = parse_args(&opt, argc, argv);
	if ((i+1) >= argc) {
		show_usage(argv[0]);
		exit(EXIT_FAILURE);
	}
	
	for (;i+1<argc;i+=2) {
		adaptive_filter(argv[i+0], argv[i+1], &opt);
	}

	return EXIT_SUCCESS;
}

static int parse_args(OPTION *dst, int argc, char **argv)
{
	int i;

	dst->width = 512;
	dst->height = 512;
	dst->threshold = 16;

	for (i=1;i<argc;i++) {

		if (argv[i][0] != '-') {
			break;
		}

		switch(argv[i][1]) {
		case 't':
			i = parse_int_option(&(dst->threshold), i, argc, argv);
			break;
		case 'w':
			i = parse_int_option(&(dst->width), i, argc, argv);
			break;
		case 'h':
			i = parse_int_option(&(dst->height), i, argc, argv);
			break;
		default:
			fprintf(stderr, "error - unknown option '-%c' is specified\n", argv[i][1]);
			return argc;
		}

	}

	return i;
}

static int parse_int_option(int *dst, int idx, int argc, char **argv)
{
	if (argv[idx][2]) {
		*dst = atoi(argv[idx]+2);
	} else if (idx+1 < argc) {
		idx += 1;
		*dst = atoi(argv[idx]);
	} else {
		fprintf(stderr, "error - option '-%c' requires additional parameter [integer]\n", argv[idx][1]);
		return argc;
	}

	return idx;
}

static void show_usage(const char *cmd)
{
	char name[1024];

	copy_basename(name, cmd);
	
	fprintf(stdout, "%s - add noise grayscale dat\n", name);
	fprintf(stdout, "usage: %s [options] src.dat dst.bmp [more pair..]\n", name);
	fprintf(stdout, "options:\n");
	fprintf(stdout, "  -w width  : data element width [int, default:512]\n");
	fprintf(stdout, "  -h height : data element height [int, default:512]\n");
	fprintf(stdout, "  -t threshold : filter threshold [int, default:16]\n");
}

static void copy_basename(char *dst, const char *src)
{
	int n;
	const char *p;
	const char *head;
	const char *tail;

	// remove directory
	p = src;
	head = src;
	tail = NULL;
	while(p[0] != 0){
		if ((p[0] == '\\') || (p[0] == '/')) {
			head = p+1;
			tail = NULL;
		} else if (p[0] == '.') {
			tail = p;
		}
		p = CharNext(p);
	}

	// copy cmd name
	if (tail == NULL) {
		tail = p;
	}

	n = tail - head;
	if(n > 1023){
		n = 1023;
	}
	
	memcpy(dst, head, n);
	dst[n] = 0;
}

unsigned char clip_u8(int val)
{
	if (val < 0) {
		return 0;
	}
	if (val > 255) {
		return 255;
	}
	return (unsigned char)val;
}

static void adaptive_filter(const char *src_path, const char *dst_path, const OPTION *opt)
{
	FILE *sfd;
	FILE *dfd;
	unsigned char *src,*ptr;
	unsigned char *dst;

	int n,m,y,x;

	sfd = NULL;
	dfd = NULL;
	src = NULL;
	dst = NULL;

	sfd = fopen(src_path, "rb");
	if (sfd == NULL) {
		fprintf(stderr, "error - failed on fopen(%s) [src]\n", src_path);
		goto LAST;
	}

	n = opt->width * opt->height;
	m = (opt->width+4) * (opt->height*4);
	src = (unsigned char *)malloc(m);
	dst = (unsigned char *)malloc(n);
	if ((src == NULL) || (dst == NULL)) {
		fprintf(stderr, "error - failed on malloc(%d)\n", n);
		goto LAST;
	}

	// マージンをハードコーディング : 望ましくないことは承知の上で
	// 読み込み [左右拡張しつつ]
	ptr = src + (2 * (opt->width+4)) + 2;
	x = opt->width-1;
	for (y=0;y<opt->height;y++) {
		if (fread(ptr, 1, opt->width, sfd) != opt->width) {
			fprintf(stderr, "error - failed on fread(%d)\n", n);
		}
		ptr[ -2] = ptr[ -1] = ptr[0];
		ptr[x+2] = ptr[x+1] = ptr[x];
		ptr += (opt->width + 4);
	}
	// 上 2 ライン拡張
	x = opt->width+4;
	memcpy(src+0, src+2*x, x);
	memcpy(src+x, src+2*x, x);
	// 下 2 ライン拡張
	ptr -= 2;
	memcpy(ptr+0, ptr-x, x);
	memcpy(ptr+x, ptr-x, x);

	ptr = src + (2 * (opt->width+4)) + 2;
	// adaptive_5x5(dst, ptr, opt->width, opt->height, 2, opt->threshold);
	adaptive_5x5_simd(dst, ptr, opt->width, opt->height, 2, opt->threshold);

	dfd = fopen(dst_path, "wb");
	if (dfd == NULL) {
		fprintf(stderr, "error - failed on fopen(%s) [dst]\n", dst_path);
		goto LAST;
	}

	if (fwrite(dst, 1, n, dfd) != n) {
		fprintf(stderr, "error - failed on fwrite(%d)\n", n);
		goto LAST;
	}

LAST:
	if (src != NULL) {
		free(src);
		src = NULL;
	}

	if (dst != NULL) {
		free(dst);
		dst = NULL;
	}

	if (sfd != NULL) {
		fclose(sfd);
		sfd = NULL;
	}

	if (dfd != NULL) {
		fclose(dfd);
		sfd = NULL;
	}
}

static int thresholding(int val, int org, int threshold)
{
	if (abs(val-org) > threshold) {
		return org;
	}

	return val;
}

static void adaptive_5x5(unsigned char *dst, const unsigned char *src, int width, int height, int margin, int threshold)
{
	int x,y;
	unsigned int w;
	int src_pitch;
	const unsigned char *nxt,*pre,*nx2,*pr2;

	// フィルタは
	// 0 1 1 1 0
	// 1 2 2 2 1
	// 1 2 4 2 1
	// 1 2 2 2 1
	// 0 1 1 1 0

	src_pitch = width + (margin*2);

	pr2 = src - src_pitch*2;
	pre = src - src_pitch;
	nxt = src + src_pitch;
	nx2 = src + src_pitch*2;

	for (y=0;y<height;y++) {
		for (x=0;x<width;x++) {

			int center = src[x];
			
			w  = thresholding(pr2[x-1], center, threshold);
			w += thresholding(pr2[x],   center, threshold);
			w += thresholding(pr2[x+1], center, threshold);

			w += thresholding(pre[x-2], center, threshold);
			w += thresholding(pre[x-1], center, threshold)*2;
			w += thresholding(pre[x],   center, threshold)*2;
			w += thresholding(pre[x+1], center, threshold)*2;
			w += thresholding(pre[x+2], center, threshold);

			w += thresholding(src[x-2], center, threshold);
			w += thresholding(src[x-1], center, threshold)*2;
			w += center                             *4;
			w += thresholding(src[x+1], center, threshold)*2;
			w += thresholding(src[x+2], center, threshold);

			w += thresholding(nxt[x-2], center, threshold);
			w += thresholding(nxt[x-1], center, threshold)*2;
			w += thresholding(nxt[x],   center, threshold)*2;
			w += thresholding(nxt[x+1], center, threshold)*2;
			w += thresholding(nxt[x+2], center, threshold);

			w += thresholding(nx2[x-1], center, threshold);
			w += thresholding(nx2[x],   center, threshold);
			w += thresholding(nx2[x+1], center, threshold);

			dst[x] = clip_u8((w+16)/32);
		}
		dst += width;
		src += src_pitch;
		pr2 += src_pitch;
		pre += src_pitch;
		nxt += src_pitch;
		nx2 += src_pitch;
	}
}

static void adaptive_5x5_8pixel_setup(const unsigned char *center)
{
	__asm {
		mov        esi, center;
		pcmpeqw    xmm1, xmm1; // all(-1)
		pxor       xmm2, xmm2; // all(0)
		pmovzxbw   xmm3, [esi];
	}
}

static void adaptive_5x5_8pixel_line_2(const unsigned char *line)
{
	// 以下 4 レジスタは外部で設定する
	// xmm0 - threshold
	// xmm1 - all(-1)
	// xmm2 - sum
	// xmm3 - center
	
	__asm {
		mov        esi, line;
		pmovzxbw   xmm4, [esi];
		pmovzxbw   xmm5, [esi+1];
		movdqa     xmm6, xmm4;
		movdqa     xmm7, xmm5;
		psubw      xmm4, xmm3; // val - center
		psubw      xmm5, xmm3;
		pabsw      xmm4, xmm4; // abs(val-center)
		pabsw      xmm5, xmm5; //
		pcmpgtw    xmm4, xmm0; // abs(val-center) > threshold : mask [center side]
		pcmpgtw    xmm5, xmm0;
		pxor       xmm4, xmm1; // mask bit inverse
		pxor       xmm5, xmm1;
		pand       xmm6, xmm4; // apply mask [val]
		pand       xmm7, xmm5;
		pxor       xmm4, xmm1; // restore mask
		pxor       xmm5, xmm1;
		pand       xmm4, xmm3; // apply mask [center]
		pand       xmm5, xmm3;
		por        xmm4, xmm6;
		pmovzxbw   xmm6, [esi+2];
		por        xmm5, xmm7;
		movdqa     xmm7, xmm6;
		paddw      xmm4, xmm5;
		psubw      xmm7, xmm3;
		paddw      xmm2, xmm4;
		pabsw      xmm7, xmm7;
		pcmpgtw    xmm7, xmm0; // mask [center side]
		movdqa     xmm4, xmm7; 
		pxor       xmm7, xmm1; // mask [val side]
		pand       xmm4, xmm3; // apply mask [center]
		pand       xmm7, xmm6; // apply mask [val]
		por        xmm4, xmm7;
		paddw      xmm2, xmm4;
	}
}

static void adaptive_5x5_8pixel_line_1(const unsigned char *line)
{
	// 以下 4 レジスタは外部で設定する
	// xmm0 - threshold
	// xmm1 - all(-1)
	// xmm2 - sum
	// xmm3 - center
	
	__asm {
		mov        esi, line;
		pmovzxbw   xmm4, [esi];   // [x-2]
		pmovzxbw   xmm5, [esi+4]; // [x+2]
		movdqa     xmm6, xmm4;
		movdqa     xmm7, xmm5;
		psubw      xmm4, xmm3; // val - center
		psubw      xmm5, xmm3;
		pabsw      xmm4, xmm4; // abs(val-center)
		pabsw      xmm5, xmm5; //
		pcmpgtw    xmm4, xmm0; // abs(val-center) > threshold : mask [center side]
		pcmpgtw    xmm5, xmm0;
		pxor       xmm4, xmm1; // invert mask bit
		pxor       xmm5, xmm1;
		pand       xmm6, xmm4; // apply mask [val]
		pand       xmm7, xmm5;
		pxor       xmm4, xmm1; // restore mask bit
		pxor       xmm5, xmm1;
		pand       xmm4, xmm3; // apply mask [center]
		pand       xmm5, xmm3;
		por        xmm4, xmm6;
		por        xmm5, xmm7;
		pmovzxbw   xmm6, [esi+1]; // [x-1]
		paddw      xmm4, xmm5;
		pmovzxbw   xmm7, [esi+3]; // [x+1]
		paddw      xmm2, xmm4;
		movdqa     xmm4, xmm6;
		movdqa     xmm5, xmm7;
		psubw      xmm6, xmm3; // val - center
		psubw      xmm7, xmm3;
		pabsw      xmm6, xmm6; // abs(val-center)
		pabsw      xmm7, xmm7;
		pcmpgtw    xmm6, xmm0; // abs(val-center) > threshold : mask [center side]
		pcmpgtw    xmm7, xmm0;
		pxor       xmm6, xmm1; // invert mask bit
		pxor       xmm7, xmm1;
		pand       xmm4, xmm6; // apply mask [val]
		pand       xmm5, xmm7;
		pxor       xmm6, xmm1; // restore mask
		pxor       xmm7, xmm1;
		pand       xmm6, xmm3;
		pand       xmm7, xmm3;
		por        xmm4, xmm6;
		pmovzxbw   xmm6, [esi+2]; // [x]
		por        xmm5, xmm7;
		movdqa     xmm7, xmm6;
		psubw      xmm6, xmm3; // val - center
		paddw      xmm4, xmm5;
		pabsw      xmm6, xmm6; // abs(val-center)
		pcmpgtw    xmm6, xmm0; // abs(val-center) > threshold : mask [center side]
		movdqa     xmm5, xmm6;
		pxor       xmm6, xmm1; // invert mask bit
		pand       xmm5, xmm3; // apply mask [center]
		pand       xmm6, xmm7; // apply mask [val]
		por        xmm5, xmm6;
		paddw      xmm4, xmm5;
		paddw      xmm4, xmm4; // (thres([x-1]) + thres([x]) + thres([x+1])) * 2
		paddw      xmm2, xmm4;
	}
}

static void adaptive_5x5_8pixel_line_0(const unsigned char *line)
{
	// 以下 4 レジスタは外部で設定する
	// xmm0 - threshold
	// xmm1 - all(-1)
	// xmm2 - sum
	// xmm3 - center
	
	__asm {
		mov        esi, line;
		pmovzxbw   xmm4, [esi];   // [x-2]
		pmovzxbw   xmm5, [esi+4]; // [x+2]
		movdqa     xmm6, xmm4;
		movdqa     xmm7, xmm5;
		psubw      xmm4, xmm3; // val - center
		psubw      xmm5, xmm3;
		pabsw      xmm4, xmm4; // abs(val-center)
		pabsw      xmm5, xmm5; //
		pcmpgtw    xmm4, xmm0; // abs(val-center) > threshold : mask [center side]
		pcmpgtw    xmm5, xmm0;
		pxor       xmm4, xmm1; // invert mask bit
		pxor       xmm5, xmm1;
		pand       xmm6, xmm4; // apply mask [val]
		pand       xmm7, xmm5;
		pxor       xmm4, xmm1; // restore mask bit
		pxor       xmm5, xmm1;
		pand       xmm4, xmm3; // apply mask [center]
		pand       xmm5, xmm3;
		por        xmm4, xmm6;
		por        xmm5, xmm7;
		pmovzxbw   xmm6, [esi+1]; // [x-1]
		paddw      xmm2, xmm4;
		pmovzxbw   xmm7, [esi+3]; // [x+1]
		paddw      xmm2, xmm5;
		movdqa     xmm4, xmm6;
		movdqa     xmm5, xmm7;
		psubw      xmm6, xmm3; // val - center
		psubw      xmm7, xmm3;
		pabsw      xmm6, xmm6; // abs(val-center)
		pabsw      xmm7, xmm7;
		pcmpgtw    xmm6, xmm0; // abs(val-center) > threshold : mask [center side]
		pcmpgtw    xmm7, xmm0;
		pxor       xmm6, xmm1; // invert mask bit
		pxor       xmm7, xmm1;
		pand       xmm4, xmm6; // apply mask [val]
		pand       xmm5, xmm7;
		pxor       xmm6, xmm1; // restore mask
		pxor       xmm7, xmm1;
		pand       xmm6, xmm3;
		pand       xmm7, xmm3;
		por        xmm4, xmm6;
		movdqa     xmm6, xmm3; // [x] : center
		por        xmm5, xmm7;
		psllw      xmm6, 2; // center*4
		paddw      xmm4, xmm5;
		paddw      xmm2, xmm6;
		paddw      xmm4, xmm4; // (thres([x-1]) + thres([x+1])) * 2
		paddw      xmm2, xmm4;
	}
}

static void adaptive_5x5_8pixel_store(unsigned char *dst)
{
	__declspec(align(16)) static const short all_16[8] = {
		16, 16, 16, 16, 16, 16, 16, 16,
	};
	
	// 以下のレジスタに加算結果が格納されている
	// xmm2 - sum
	
	__asm {
		mov        esi, dst;
		paddw      xmm2, all_16;
		psraw      xmm2, 5;
		packuswb   xmm2, xmm2;
		movq       qword ptr [esi], xmm2;
	}
}

static void adaptive_5x5_simd(unsigned char *dst, const unsigned char *src, int width, int height, int margin, int threshold)
{
	int x,y;
	int src_pitch;
	const unsigned char *top;
	const unsigned char *ptr;

	if ((width & 7) != 0) {
		fprintf(stderr, "error - unsupported width=%d\n", width);
		return;
	}

	src_pitch = width + (margin*2);

	top = src - (src_pitch * 2 + 2);

	__asm { // 閾値を読み込んで変換
		movd       xmm0, dword ptr threshold;
		pshufd     xmm0, xmm0, 00000000b;
		packssdw   xmm0, xmm0;
	}

	for (y=0;y<height;y++) {
		for (x=0;x<width;x+=8) {

			ptr = top + x;

			adaptive_5x5_8pixel_setup(src+x);
			
			adaptive_5x5_8pixel_line_2(ptr+1);
			ptr += src_pitch;
			adaptive_5x5_8pixel_line_1(ptr);
			ptr += src_pitch;
			adaptive_5x5_8pixel_line_0(ptr);
			ptr += src_pitch;
			adaptive_5x5_8pixel_line_1(ptr);
			ptr += src_pitch;
			adaptive_5x5_8pixel_line_2(ptr+1);

			adaptive_5x5_8pixel_store(dst+x);
		}

		dst += width;
		src += src_pitch;
		top += src_pitch;
	}
}
