#define _BSD_SOURCE
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gdk-pixbuf/gdk-pixbuf.h>

struct mat2 {
	float a1, a2, b1, b2;
	float mc;
};

static inline float
det(struct mat2 *m)
{
	return m->a1 * m->b2 - m->a2 * m->b1;
}

static inline float
trace(struct mat2 *m)
{
	return m->a1 + m->b2;
}

static inline void
pixbuf_get(GdkPixbuf *pixbuf, guchar **pixels,
	   gint *width, gint *height, gint *nch, gint *stride)
{
	if (pixels) *pixels = gdk_pixbuf_get_pixels(pixbuf);
	if (width)  *width  = gdk_pixbuf_get_width(pixbuf);
	if (height) *height = gdk_pixbuf_get_height(pixbuf);
	if (nch)    *nch    = gdk_pixbuf_get_n_channels(pixbuf);
	if (stride) *stride = gdk_pixbuf_get_rowstride(pixbuf);
}

static void
grey_scale(GdkPixbuf *image)
{
	gint Y, x, y, width, height, nch, stride;
	guchar *p, *row;

	pixbuf_get(image, &row, &width, &height, &nch, &stride);
	for (y = 0; y < height; ++y, row += stride) {
		for (x = 0, p = row; x < width; ++x, p += nch) {
			Y = 0.299 * p[0] + 0.587 * p[1] + 0.115 * p[2];
			p[0] = p[1] = p[2] = Y;
		}
	}
}

int *
conv(GdkPixbuf *pixbuf, gint *operator, gint block_size)
{
	gint Y, i, j, ci, cj, x, y, width, height, nch, stride;
	guchar *p, *row, v;
	int *out;
	
	pixbuf_get(pixbuf, &row, &width, &height, &nch, &stride);

	out = malloc(width * height * sizeof *out);
	if (!out)
		return NULL;
	for (y = 0; y < height; ++y, row += stride) {
		for (x = 0, p = row; x < width; ++x, p += nch) {
			Y = 0;
			for (j = 0; j < block_size; ++j) {
				cj = CLAMP(j - block_size/2, -y, height-1-y);
				for (i = 0; i < block_size; ++i) {
					ci = CLAMP(i - block_size/2, -x, width-1-x);

					v = p[cj * stride + ci * nch + 0];
					Y += v * operator[j*block_size + i];
				}
			}
			*out++ = Y/4;
		}
	}

	return out - width*height;
}

struct mat2 *
multiply(int *deriv_x, int *deriv_y, int width, int height)
{
	struct mat2 *m, *pm;
	int *px, *py;
	int i, n;

	n = width * height;
	m = calloc(n, sizeof *m);
	if (!m)
		return NULL;
	px = deriv_x;
	py = deriv_y;
	pm = m;
	for (i = 0; i < n; ++i) {
		pm->a1          = *px * *px;
		pm->a2 = pm->b1 = *px * *py;
		pm->b2          = *py * *py;

		++px; ++py; ++pm;
	}

	return m;
}

struct mat2 *
calc_window_sum(struct mat2 *m, int width, int height, int wsize)
{
	struct mat2 *avg, *v;
	float w;
	int x, y, i, j;

	avg = calloc(width * height, sizeof *avg);
	if (!avg)
		return NULL;

	for (y = 0; y < height; ++y) {
		for (x = 0; x < width; ++x) {
			for (j = -wsize/2; j < wsize/2; ++j) {
				for (i = -wsize/2; i < wsize/2; ++i) {
					v = &m[(CLAMP(y+j, 0, height-1) * width
						+ CLAMP(x+i, 0, width-1))];
					w = (M_1_PI * 1.0 / (2.0 * 0.16) *
					     exp(- i*i - j*j / 0.16));

					avg->a1 += w * v->a1;
					avg->a2 += w * v->a2;
					avg->b1 += w * v->b1;
					avg->b2 += w * v->b2;
				}
			}
			++avg;
		}
	}

	return avg - width*height;
}

gboolean
check_neighbours(struct mat2 *m, int x, int y, int width, int height)
{
	int i, j;
	gboolean max = TRUE;
	int n = 2;

	for (j = MAX(0, y-n); j < MIN(y+n, height-1); ++j) {
		for (i = MAX(0, x-n); i < MIN(x+n, width-1); ++i) {
			if (m[j * width + i].mc > m[y * width + x].mc)
				max = FALSE;
		}
	}

	return max;
}

int
main(int argc, char *argv[])
{
	GdkPixbuf *image, *grey;
	int *deriv_x, *deriv_y;
	gint sobel_x[] = {
		1, 0, -1,
		2, 0, -2,
		1, 0, -1
	};
	gint sobel_y[] = {
		1,  2,  1,
		0,  0,  0,
		-1, -2, -1
	};
	const float k = 0.04;
	int x, y, width, height, size, nch, stride;
	struct mat2 *m, *n, *c;
	guchar *p, *row;
	float max_mc, threshold;

	if (argc < 2)
		return 1;

	g_type_init();
	image = gdk_pixbuf_new_from_file(argv[1], NULL);
	if (!image)
		return 1;
	width = gdk_pixbuf_get_width(image);
	height = gdk_pixbuf_get_height(image);

	grey = gdk_pixbuf_copy(image);
	grey_scale(grey);

	deriv_x = conv(grey, sobel_x, 3);
	deriv_y = conv(grey, sobel_y, 3);
	gdk_pixbuf_save(grey, argc > 3 ? argv[3] : "grey.tiff", "tiff",
			NULL, NULL);

	m = multiply(deriv_x, deriv_y, width, height);
	n = calc_window_sum(m, width, height, 9);
	free(m);

	max_mc = 0;
	size = width * height;
	for (c = n; c < n+size; ++c) {
		c->mc = det(c) - k * trace(c) * trace(c);
		if (c->mc > max_mc)
			max_mc = c->mc;
	}
	printf("max_mc: %f\n", max_mc);

	threshold = 0.001 * max_mc;
	pixbuf_get(grey, &row, &width, &height, &nch, &stride);
	for (y = 0; y < height; ++y, row += stride) {
		for (x = 0, p = row; x < width; ++x, p += nch) {
			if (n[y * width + x].mc > threshold &&
			    check_neighbours(n, x, y, width, height)) {
				p[0] = 255;
				p[1] = 0;
				p[2] = 0;
				printf("corner at: %d, %d\n", x, y);
			}
		}
	}

	gdk_pixbuf_save(grey, argc > 2 ? argv[2] : "harris.tiff", "tiff",
			NULL, NULL);

	free(n);
	free(deriv_x);
	free(deriv_y);
	g_object_unref(image);
	g_object_unref(grey);

	return 0;
}