#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <unistd.h>
#include <fcntl.h>
#include <grass/imagery.h>
#include <grass/glocale.h>
#include "global.h"
#include "crs.h"

struct Stats
{
    double x, y, z, g;
    double sum2, rms;
};

static void update_stats(struct Stats *st, int n,
                         double dx, double dy, double *dz,
			 double dg, double d2)
{
    st->x += dx;
    st->y += dy;
    if (dz)
	st->z += *dz;
    st->g += dg;
    st->sum2 += d2;
}

static void diagonal(double *dg, double *d2, double dx, double dy, double *dz)
{
    *d2 = dx * dx + dy * dy;
    if (dz)
	*d2 += *dz * *dz;
    *dg = sqrt(*d2);
}


static void compute_rms(struct Control_Points *cp, struct Control_Points_3D *cp3,
                        int order, int use3d, int orthorot, char *sep, FILE *fp)
{
    int n;
    int count, npoints;
    struct Stats fwd, rev;

    fwd.sum2 = fwd.rms = rev.sum2 = rev.rms = 0.0;
    
    fwd.x = fwd.y = fwd.z = fwd.g = 0.0;
    rev.x = rev.y = rev.z = rev.g = 0.0;

    count = 0;
    
    /* print index, forward difference, backward difference, 
     * forward rms, backward rms */
    if (use3d)
	fprintf(fp, "index%sfwd_dx%sfwd_dy%sfwd_dz%sback_dx%sback_dy%sback_dz%sfwd_RMS%sback_RMS",
               sep, sep, sep, sep, sep, sep, sep, sep);
    else
	fprintf(fp, "index%sfwd_dx%sfwd_dy%sback_dx%sback_dy%sfwd_RMS%sback_RMS",
	       sep, sep, sep, sep, sep, sep);

    fprintf(fp, "\n");
    
    if (use3d)
	npoints = cp3->count;
    else
	npoints = cp->count;

    for (n = 0; n < npoints; n++) {
	double e1, n1, z1, e2, n2, z2;
	double fx, fy, fz, fd, fd2;
	double rx, ry, rz, rd, rd2;

	if (use3d || orthorot) {
	    if (cp3->status[n] <= 0)
		continue;
	}
	else {
	    if (cp->status[n] <= 0)
		continue;
	}

	count++;

	/* forward: source -> target */
	if (use3d) {
	    if (orthorot)
		CRS_georef_or(cp3->e1[n], cp3->n1[n], cp3->z1[n],
			      &e2, &n2, &z2, OR12);
	    else
		CRS_georef_3d(cp3->e1[n], cp3->n1[n], cp3->z1[n],
			      &e2, &n2, &z2, 
			      E12, N12, Z12, 
			      order);

	    fx = fabs(e2 - cp3->e2[n]);
	    fy = fabs(n2 - cp3->n2[n]);
	    fz = fabs(z2 - cp3->z2[n]);

	    diagonal(&fd, &fd2, fx, fy, &fz);

	    update_stats(&fwd, n, fx, fy, &fz, fd, fd2);
	}
	else {
	    I_georef(cp->e1[n], cp->n1[n], &e2, &n2, E12, N12, order);

	    fx = fabs(e2 - cp->e2[n]);
	    fy = fabs(n2 - cp->n2[n]);

	    diagonal(&fd, &fd2, fx, fy, NULL);

	    update_stats(&fwd, n, fx, fy, NULL, fd, fd2);
	}

	/* backward: target -> source */
	if (use3d) {
	    if (orthorot)
		CRS_georef_or(cp3->e2[n], cp3->n2[n], cp3->z2[n],
			      &e1, &n1, &z1, OR21);
	    else
		CRS_georef_3d(cp3->e2[n], cp3->n2[n], cp3->z2[n],
			      &e1, &n1, &z1,
			      E21, N21, Z21,
			      order);

	    rx = fabs(e1 - cp3->e1[n]);
	    ry = fabs(n1 - cp3->n1[n]);
	    rz = fabs(z1 - cp3->z1[n]);

	    diagonal(&rd, &rd2, rx, ry, &rz);

	    update_stats(&rev, n, rx, ry, &rz, rd, rd2);
	}
	else {
	    I_georef(cp->e2[n], cp->n2[n], &e1, &n1, E21, N21, order);

	    rx = fabs(e1 - cp->e1[n]);
	    ry = fabs(n1 - cp->n1[n]);

	    diagonal(&rd, &rd2, rx, ry, NULL);

	    update_stats(&rev, n, rx, ry, NULL, rd, rd2);
	}

	/* print index, forward difference, backward difference, 
	 * forward rms, backward rms */
	fprintf(fp, "%d", n + 1);
	fprintf(fp, "%s%f%s%f", sep, fx, sep, fy);
	if (use3d)
	    fprintf(fp, "%s%f", sep, fz);
	fprintf(fp, "%s%f%s%f", sep, rx, sep, ry);
	if (use3d)
	    fprintf(fp, "%s%f", sep, rz);
	fprintf(fp, "%s%.4f", sep, fd);
	fprintf(fp, "%s%.4f", sep, rd);

	fprintf(fp, "\n");
    }

    if (count > 0) {
	fwd.x /= count;
	fwd.y /= count;
	fwd.g /= count;
	rev.x /= count;
	rev.y /= count;
	rev.g /= count;
	if (use3d) {
	    fwd.z /= count;
	    rev.z /= count;
	}
	fwd.rms = sqrt(fwd.sum2 / count);
	rev.rms = sqrt(rev.sum2 / count);
    }
    fprintf(fp, "%d", count);
    fprintf(fp, "%s%f%s%f", sep, fwd.x, sep, fwd.y);
    if (use3d)
	fprintf(fp, "%s%f", sep, fwd.z);
    fprintf(fp, "%s%f%s%f", sep, rev.x, sep, rev.y);
    if (use3d)
	fprintf(fp, "%s%f", sep, rev.z);
    fprintf(fp, "%s%.4f", sep, fwd.rms);
    fprintf(fp, "%s%.4f", sep, rev.rms);

    fprintf(fp, "\n");
}


int new_control_point_3d(struct Control_Points_3D *cp,
			double e1, double n1, double z1,
			double e2, double n2, double z2,
			int status)
{
    int i;
    unsigned int size;

    if (status < 0)
	return 1;

    i = (cp->count)++;
    size = cp->count * sizeof(double);
    cp->e1 = (double *)G_realloc(cp->e1, size);
    cp->e2 = (double *)G_realloc(cp->e2, size);
    cp->n1 = (double *)G_realloc(cp->n1, size);
    cp->n2 = (double *)G_realloc(cp->n2, size);
    cp->z1 = (double *)G_realloc(cp->z1, size);
    cp->z2 = (double *)G_realloc(cp->z2, size);
    size = cp->count * sizeof(int);
    cp->status = (int *)G_realloc(cp->status, size);

    cp->e1[i] = e1;
    cp->e2[i] = e2;
    cp->n1[i] = n1;
    cp->n2[i] = n2;
    cp->z1[i] = z1;
    cp->z2[i] = z2;
    cp->status[i] = status;

    return 0;
}

static int read_control_points(FILE * fd, struct Control_Points *cp)
{
    char buf[1000];
    double e1, e2, n1, n2;
    int status;

    cp->count = 0;

    /* read the control point lines. format is:
       image_east image_north  target_east target_north  status
     */
    cp->e1 = NULL;
    cp->e2 = NULL;
    cp->n1 = NULL;
    cp->n2 = NULL;
    cp->status = NULL;

    while (G_getl2(buf, sizeof buf, fd)) {
	G_strip(buf);
	if (*buf == '#' || *buf == 0)
	    continue;
	if (sscanf(buf, "%lf%lf%lf%lf%d", &e1, &n1, &e2, &n2, &status) == 5)
	    I_new_control_point(cp, e1, n1, e2, n2, status);
	else
	    return -4;
    }

    return 1;
}

static int read_control_points_3d(FILE * fd, struct Control_Points_3D *cp)
{
    char buf[1000];
    double e1, e2, n1, n2, z1, z2;
    int status;

    cp->count = 0;

    /* read the control point lines. format is:
       source_east source_north source_height  target_east target_north target_height  status
     */
    cp->e1 = NULL;
    cp->e2 = NULL;
    cp->n1 = NULL;
    cp->n2 = NULL;
    cp->z1 = NULL;
    cp->z2 = NULL;
    cp->status = NULL;

    while (G_getl2(buf, sizeof buf, fd)) {
	G_strip(buf);
	if (*buf == '#' || *buf == 0)
	    continue;
	if (sscanf(buf, "%lf%lf%lf%lf%lf%lf%d", &e1, &n1, &z1, &e2, &n2, &z2, &status) == 7)
	    new_control_point_3d(cp, e1, n1, z1, e2, n2, z2, status);
	else
	    return -4;
    }

    return 1;
}


int get_control_points(char *group, char *pfile, int order, int use3d, 
                       int orthorot, int rms, char *sep, FILE *fpr)
{
    char msg[200];
    struct Control_Points cp;
    struct Control_Points_3D cp3;
    int ret = 0;
    int order_pnts[2][3] = {{ 3, 6, 10 }, { 4, 10, 20 }};
    
    cp.count = cp3.count = 0;
    cp.e1 = cp.e2 = cp3.e1 = cp3.e2 = NULL;
    cp.n1 = cp.n2 = cp3.n1 = cp3.n2 = NULL;
    cp3.z1 = cp3.z2 = NULL;
    cp.status = cp3.status = NULL;
    
    msg[0] = '\0';

    if (use3d) {
	/* read 3D GCPs from points file */
	FILE *fp;
	int fd, stat;
	
	if ((fd = open(pfile, 0)) < 0)
	    G_fatal_error(_("Can not open file <%s>"), pfile);
	    
	fp = fdopen(fd, "r");

	stat = read_control_points_3d(fp, &cp3);
	fclose(fp);
	if (stat < 0) {
	    G_fatal_error(_("Bad format in control point file <%s>"),
		      pfile);
	    return 0;
	}

	if (orthorot)
	    ret = CRS_compute_georef_equations_or(&cp3, OR12, OR21);
	else
	    ret = CRS_compute_georef_equations_3d(&cp3, E12, N12, Z12,
	                                          E21, N21, Z21, order);
    }
    else if (pfile) {
	/* read 2D GCPs from points file */
	FILE *fp;
	int fd, stat;
	
	if ((fd = open(pfile, 0)) < 0)
	    G_fatal_error(_("Can not open file <%s>"), pfile);
	    
	fp = fdopen(fd, "r");

	stat = read_control_points(fp, &cp);
	fclose(fp);
	if (stat < 0) {
	    G_fatal_error(_("Bad format in control point file <%s>"),
		      pfile);
	    return 0;
	}

	ret = I_compute_georef_equations(&cp, E12, N12, E21, N21, order);
    }
    else {
	/* read group control points */
	if (!I_get_control_points(group, &cp))
	    exit(0);

	sprintf(msg, _("Control Point file for group <%s@%s> - "),
		group, G_mapset());
		
	ret = I_compute_georef_equations(&cp, E12, N12, E21, N21, order);
    }

    switch (ret) {
    case 0:
	sprintf(&msg[strlen(msg)],
		_("Not enough active control points for current order, %d are required."),
		(orthorot ? 3 : order_pnts[use3d != 0][order - 1]));
	break;
    case -1:
	strcat(msg, _("Poorly placed control points."));
	strcat(msg, _(" Can not generate the transformation equation."));
	break;
    case -2:
	strcat(msg, _("Not enough memory to solve for transformation equation"));
	break;
    case -3:
	strcat(msg, _("Invalid order"));
	break;
    default:
	break;
    }
    if (ret != 1)
        G_fatal_error("%s", msg);
	
    if (rms) {
	compute_rms(&cp, &cp3, order, use3d, orthorot, sep, fpr);
    }
    
    return 1;
}