#include "image.h"
#include <memdraw.h>
#include <bio.h>

typedef double Matrix[3][3];

uchar α0[4];

int
parsemat(Matrix m, char *s)
{
	char *a[10];
	double *f;
	int n, i;

	if((n = tokenize(s, a, nelem(a))) != 9) {
		werrstr("%d/9", n);
		return -1;
	}

	f = &m[0][0];
	for(i = 0; i < 9; i++)
		*f++ = atof(a[i]);
	return 0;
}

double
determinate(Matrix m)
{
	return m[0][0]*m[1][1]*m[2][2]
		+ m[0][1]*m[1][2]*m[2][0]
		+ m[0][2]*m[1][0]*m[2][1]
		- m[0][0]*m[1][2]*m[2][1]
		- m[0][1]*m[1][0]*m[2][2]
		- m[0][2]*m[1][1]*m[2][0];
}

void
adjoint(Matrix a, Matrix m)
{
	a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];
	a[0][1] = m[2][1]*m[0][2] - m[2][2]*m[0][1];
	a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];

	a[1][0] = m[1][2]*m[2][0] - m[1][0]*m[2][2];
	a[1][1] = m[2][2]*m[0][0] - m[2][0]*m[0][2];
	a[1][2] = m[0][2]*m[1][0] - m[0][0]*m[1][2];

	a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];
	a[2][1] = m[2][0]*m[0][1] - m[2][1]*m[0][0];
	a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0];
}

double
invert(Matrix inv, Matrix m)
{
	double d, dinv;
	int i, j;

	adjoint(inv, m);
	d = determinate(m);

	if(d != 0.0) {
		dinv = 1.0/d;
		for(i = 0; i < 3; i++) {
			for(j = 0; j < 3; j++)
				inv[i][j] *= dinv;
		}
	}
	return d;
}

Point
transform(Point p, Matrix m)
{
	Point q;

	q.x = p.x*m[0][0] + p.y*m[0][1] + 1*m[0][2];
	q.y = p.x*m[1][0] + p.y*m[1][1] + 1*m[1][2];
	return q;
}

Rectangle
transformbbox(Rectangle r, Matrix m)
{
	Rectangle r1, r2;

	r1 = canonrect(Rpt(transform(r.min, m), transform(r.max, m)));
	r2 = canonrect(Rpt(transform(Pt(r.min.x, r.max.y), m), transform(Pt(r.max.x, r.min.y), m)));
	combinerect(&r1, r2);
	return r1;
}

Rectangle
transformrect(Rectangle r, Matrix m)
{
	return canonrect(Rpt(transform(r.min, m), transform(r.max, m)));
}

void
test(Matrix m)
{
	int i;
	Point p;

	if(fmtinstall('P', Pfmt) < 0)
		sysfatal("fmtinstall");

	for(i = 0; i < 10; i++) {
		p = Pt(0, i);
		print("%P	%P\n", p, transform(p, m));
	}
}

void
usage(void)
{
	fprint(2, "usage: %s matrix <image.bit\n", argv0);
	exits("usage");
}

void
main(int argc, char **argv)
{
	Biobuf b;
	Matrix m, minv;
	Rectangle r;
	Memimage *srcmem;
	ImgHdr hsrc, hdst;
	int nc, linesz, y, x;
	uchar *dstline, *dst, *src;
	Point dstpt, srcpt;

	ARGBEGIN {
	default:
		usage();
	} ARGEND
	if(argc != 1)
		usage();

	memimageinit();
	if(fmtinstall('H', Hfmt) < 0 || fmtinstall('P', Pfmt) < 0 || fmtinstall('R', Rfmt) < 0)
		sysfatal("fmtinstall");

	if(Binit(&b, 1, OWRITE) < 0)
		sysfatal("Binit");

	if(parsemat(m, argv[0]) < 0)
		sysfatal("%r");
	invert(minv, m);

	if((srcmem = readmemimage(0)) == nil)
		sysfatal("%r");
	r = transformbbox(srcmem->r, m);
	m[0][2] -= r.min.x;
	m[1][2] -= r.min.x;
	invert(minv, m);

	hsrc = (ImgHdr){0, srcmem->chan, srcmem->r};
	hdst = (ImgHdr){0, srcmem->chan, transformbbox(srcmem->r, m)};
	nc = chantodepth(hdst.chan) / 8;
	linesz = Dx(hdst.r) * nc;
	if((dstline = malloc(linesz)) == nil)
		sysfatal("malloc line: %d: %r", linesz);

	Bprint(&b, "%H", hdst);
	for(y = hdst.r.min.y; y < hdst.r.max.y; y++) {
		for(x = 0; x < Dx(hdst.r); x++) {
			dstpt = Pt(hdst.r.min.x + x, y);
			srcpt = transform(dstpt, minv);
			dst = dstline + x*nc;

			if(ptinrect(srcpt, hsrc.r)) {
				src = byteaddr(srcmem, srcpt);
				Bwrite(&b, src, nc);
			} else {
				Bwrite(&b, α0, nc);
			}
		}
	}

	exits(nil);
}