/* samplerect.cpp - downsample check image based on corner coordinates
 * second version - uses homography
 */

static char usage[] = "usage: %s coords.txt in.png out.png width\n";

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "imageLib.h"

#define RATIO24 1.525
#define RATIO140 1.402

// defined in GaussElim.cpp
void GaussElimFullPivot(double M[8][9], int n, int m, double* X);

void computeHomography(int x[], int y[], int u[], int v[], double h[])
{
    double M[8][9] =
	{{x[0], y[0], 1, 0, 0, 0, -u[0]*x[0], -u[0]*y[0], u[0]},
	 {x[1], y[1], 1, 0, 0, 0, -u[1]*x[1], -u[1]*y[1], u[1]},
	 {x[2], y[2], 1, 0, 0, 0, -u[2]*x[2], -u[2]*y[2], u[2]},
	 {x[3], y[3], 1, 0, 0, 0, -u[3]*x[3], -u[3]*y[3], u[3]},
	 {0, 0, 0, x[0], y[0], 1, -v[0]*x[0], -v[0]*y[0], v[0]},
	 {0, 0, 0, x[1], y[1], 1, -v[1]*x[1], -v[1]*y[1], v[1]},
	 {0, 0, 0, x[2], y[2], 1, -v[2]*x[2], -v[2]*y[2], v[2]},
	 {0, 0, 0, x[3], y[3], 1, -v[3]*x[3], -v[3]*y[3], v[3]}};
    if (0) {
	for (int i=0; i<8; i++) {
	    for (int j=0; j < 9; j++)
		printf(j<6? "%5d " : "%9d ", (int)M[i][j]);
	    printf("\n");
	}
    }
    GaussElimFullPivot(M, 8, 9, h);

    if (0) {
	printf("H = \n");
	printf("%5.3f %5.3f %5.3f\n", h[0], h[1], h[2]);
	printf("%5.3f %5.3f %5.3f\n", h[3], h[4], h[5]);
	printf("%5.3f %5.3f %5.3f\n", h[6], h[7], 1.0);
    }
}

void applyH(double h[], int x, int y, int &x2, int &y2)
{
    double u = h[0]*x + h[1]*y + h[2];
    double v = h[3]*x + h[4]*y + h[5];
    double w = h[6]*x + h[7]*y + 1.0;
    x2 = int(u/w + .5); // round (assume positive coords...)
    y2 = int(v/w + .5);
}


int main(int argc, char **argv)
{
    int verbose = 1;
    if (argc != 5) {
	fprintf(stderr, usage, argv[0]);
	exit(1);
    }
    int optind = 1;
    char *coordname = argv[optind++];
    char *imname = argv[optind++];
    char *outname = argv[optind++];
    int imw = atoi(argv[optind++]);

    try {
	FILE *fp = fopen(coordname, "r");
	int x[4], y[4];
	for (int k = 0, *px = x, *py = y; k < 4; k++, px++, py++)
	    fscanf(fp, "%d %d", px, py);
	fclose(fp);
	int w1 = x[1] - x[0];
	int w2 = x[3] - x[2];
	int h1 = y[2] - y[0];
	int h2 = y[3] - y[1];
	float r11 = (float)w1/h1;
	float r12 = (float)w1/h2;
	float r21 = (float)w2/h1;
	float r22 = (float)w2/h2;
	float r = (r11 + r12 + r21 + r22) / 4.0;
	printf("aspect ratios:\n %.3f\n %.3f\n %.3f\n %.3f\navg\n %.3f \n", r11, r12, r21, r22, r);
	float ratio;
	if (fabs(r - RATIO24) < fabs(r - RATIO140)) {
	    printf("24 checkers: using ratio %g  ", RATIO24);
	    ratio = RATIO24;
	} else {
	    printf("140 checkers: using ratio %g  ", RATIO140);
	    ratio = RATIO140;
	}
	int imh = (int)(imw / ratio);
	printf("image size: %d x %d\n", imw, imh);

	int xn[] = {0, imw, 0, imw};
	int yn[] = {0, 0, imh, imh};
	double h[8];
	computeHomography(xn, yn, x, y, h); // map new to old coords

	CByteImage im;
	ReadImageVerb(im, imname, verbose);
	CShape shim = im.Shape();
	int nB = shim.nBands;
	CShape sh(imw, imh, nB);
	CByteImage outim(sh);

	for (int oy = 0; oy < imh; oy++) {
	    for (int ox = 0; ox < imw; ox++) {
		int xx, yy;
		applyH(h, ox, oy, xx, yy);
		for (int b = 0; b < nB; b++)
		    outim.Pixel(ox, oy, b) = im.Pixel(xx, yy, b);
	    }
	}

	WriteImageVerb(outim, outname, verbose);
    }
    catch (CError &err) {
	fprintf(stderr, err.message);
	fprintf(stderr, "\n");
	exit(1);
    }

    return 0;
}