#include	<stdio.h>
#include	<string.h>
#include	<stdlib.h>
#include	<malloc.h>
#include	<math.h>
#include	<conio.h>




/* Maximum number of object points. Change as needed */
#define		MAXPTS		100

#define		M_PI		3.141592653589793238

/* Macro to access upper triangular matrix stored as 1D array */
#define		INDUT(i,j)	( (((j) * ((j)-1)) / 2) + i )




typedef struct {
	double	m11, m12, m13,
			m21, m22, m23,
			m31, m32, m33,
			so, sp, sk,
			co, cp, ck,
			omega, phi, kappa,
			xl, yl, zl, f;
} PhoParamType;


typedef struct {
	double	xleft, yleft, xleft_res, yleft_res,
			xright, yright, xright_res, yright_res,
			X, Y, Z;
	char	name[24];
} PointType;




void ReadData( char *rootname, PhoParamType *Pphoto, PointType *points, int *Pnumpts );
void AllocateNormals( double **Pnorm, double **Prhs, int numpts );
void solve( double *a, double *b, int n, int invflag );
void RotationMatrix(PhoParamType *Pphoto);
double FormNormals( double *norm, double *rhs, PhoParamType leftphoto, 
		PhoParamType rightphoto, PointType *points, int numpts );
void AuxVars( PointType point, PhoParamType photo, double *Pdx, double *Pdy,
			 double *Pdz, double *Pr, double *Ps, double *Pq );
void PartXLYLZL( double part[3][7], PhoParamType photo, double r, double s, double q );
double EpsRes( double *epsilon, double x, double y, double *Pxres, double *Pyres,
			  double f, double r, double s, double q);
void AddCorrections( PhoParamType *Prightphoto, PointType *points, double *rhs, 
					int numpts, int iter, double s0, FILE *itfile );
void OutputResults( char *rootname, PhoParamType leftphoto, PhoParamType rightphoto,
				   PointType *points, double *norm, double s0, int numpts );
void pause(void);




void main(void)
{
	char			rootname[50], filename[50];
	FILE			*itfile;
	PhoParamType	leftphoto, rightphoto;
	PointType		points[MAXPTS];
	int				numpts, iter=0, converge=0, diverge=0;
	double			*norm, *rhs, s0=1.0e35, s0old;

	ReadData( rootname, &rightphoto, points, &numpts );

	AllocateNormals( &norm, &rhs, numpts );

	/* set parameters for left photo */
	leftphoto.omega = 0;
	leftphoto.phi = 0;
	leftphoto.kappa = 0;
	leftphoto.xl = 0;
	leftphoto.yl = 0;
	leftphoto.zl = rightphoto.zl;
	leftphoto.f = rightphoto.f;
	RotationMatrix(&leftphoto);	

	/* Open file for iteration results */
	strcpy(filename, rootname);
	strcat(filename, ".itr");
	itfile = fopen(filename, "w");

	do {
		iter++;			/* Increment iteration count */
		s0old = s0;		/* Save former value of s0 for convergence test */
		RotationMatrix( &rightphoto); /* Compute rotation matrix elements */
		s0 = FormNormals(norm, rhs, leftphoto, rightphoto, points, numpts);
		printf("ITERATION %d     S0 = %6.5lf\n", iter, s0);

		/* Check for convergence or divergence */
		if (fabs(s0old-s0)/s0 < 0.0001) converge=1;
		else if (s0 > s0old) diverge=1;

		/* Solve for corrections
		   If converged or diverged call for inverse also */
		solve(norm, rhs, 5+numpts*3, converge|diverge);
		AddCorrections( &rightphoto, points, rhs, numpts, iter, s0, itfile );
	} while (!converge && !diverge);

	OutputResults( rootname, leftphoto, rightphoto, points, norm, s0, numpts );
	fclose(itfile);
	pause();
}





void ReadData( char *rootname, PhoParamType *Pphoto, PointType *points, int *Pnumpts )
{
	char	filename[50], tempstr[50];
	FILE	*infile;
	int		NumRead, i;
	double	xl, yl, xr, yr, ParallaxSum;

	printf("Enter root name of relative orientation data file (.dat assumed) --> ");
	scanf("%s", rootname);
	strcpy(filename, rootname);
	strcat(filename, ".dat");
	printf("\n");

	infile = fopen(filename, "r");
	if (infile == NULL) {
		printf("ERROR. Could not open file %s\n", filename);
		pause();
		exit(1);
	}

	/* read focal length */
	fscanf(infile, "%lf", &Pphoto->f);

	/* read points until end of file is reached */
	*Pnumpts = 0;
	do {
		NumRead = fscanf(infile, "%s %lf %lf %lf %lf", tempstr, &xl, &yl, &xr, &yr);
		if (NumRead == 5) {
			/* check for array overflow */
			if (*Pnumpts == MAXPTS) {
				printf("ERROR. More than %d points in data file\n", MAXPTS);
				pause();
				exit(1);
			}
			/* store data for this point and increase count */
			strcpy( points[*Pnumpts].name, tempstr );
			points[*Pnumpts].xleft = xl;
			points[*Pnumpts].yleft = yl;
			points[*Pnumpts].xright = xr;
			points[*Pnumpts].yright = yr;
			/* set initial approximations for object coordinates */
			points[*Pnumpts].X = xl;
			points[*Pnumpts].Y = yl;
			points[*Pnumpts].Z = 0;
			(*Pnumpts)++;

		}
	} while (NumRead == 5);

	fclose(infile);

	/* Quit if not enough pass points */
	if (*Pnumpts < 5) {
		printf("ERROR. Fewer than 5 pass points in data file\n");
		pause();
		exit(1);
	}

	/* set approximations for right photo exterior orientation parameters */
	Pphoto->omega = 0;
	Pphoto->phi = 0;
	Pphoto->kappa = 0;
	Pphoto->yl = 0;
	Pphoto->zl = Pphoto->f;

	/* Set constraint for XL-right based on average parallax of object points.
	   (assumes flight-line axis coordinates) */
	ParallaxSum=0;
	for (i=0; i<*Pnumpts; i++)
		ParallaxSum += points[i].xleft - points[i].xright;
	Pphoto->xl = ParallaxSum / *Pnumpts;

}





void AllocateNormals( double **Pnorm, double **Prhs, int numpts )
{
	int		numunk = 3*numpts + 5;

	*Pnorm = calloc( numunk*(numunk+1)/2+1, sizeof(double) );
	*Prhs = calloc( numunk+1, sizeof(double) );
	if ( (*Pnorm == NULL) || (*Prhs == NULL) ) {
		printf("Dynamic memory allocation failure.\n");
		pause();
		exit(1);
	}
}





void solve( double *a, double *b, int n, int invflag )
/* Solution and inverse by recursive partitioning for upper triangular
   normal equation matrix. When complete, array 'b' is overwritten by
   solution. If 'invflag' is true (i.e. non-zero) then array 'a' is
   overwritten by the inverse also.
*/
{
	int		piv, j, i;
	double	r, *s;

	/* Allocate scratch array for inverse if needed */
	if (invflag) {
		s = calloc(n+1, sizeof(double));
		if (s == NULL) {
			printf("ERROR. Insufficient memory.\n");
			pause();
			exit(1);
		}
	}

	/* Forward elimination */
	for (piv=1; piv<=n; piv++) {
		for (i=piv+1; i<=n; i++) {
			for (j=i; j<=n; j++)
				a[INDUT(i,j)] -= a[INDUT(piv,i)] * a[INDUT(piv,j)]/a[INDUT(piv,piv)];
			b[i] -= a[INDUT(piv,i)] * b[piv]/a[INDUT(piv,piv)];
		}
	}
	/* Back substitution and inverse (if requested) */
	for (piv=n; piv>0; piv--) {
		for (j=piv+1; j<=n; j++) b[piv] -= a[INDUT(piv,j)]*b[j];
		b[piv] /= a[INDUT(piv,piv)];
		if (invflag) {
			for (j=piv+1; j<=n; j++) {
				s[j] = 0.0;
				for (i=piv+1; i<=j; i++) s[j] -= a[INDUT(piv,i)]*a[INDUT(i,j)];
				for (i=j+1; i<=n; i++) s[j] -= a[INDUT(piv,i)]*a[INDUT(j,i)];
				s[j] /= a[INDUT(piv,piv)];
			}
			r = 1.0;
			for (j=piv+1; j<=n; j++) {
				r -= a[INDUT(piv,j)]*s[j];
				a[INDUT(piv,j)] = s[j];
			}
			a[INDUT(piv,piv)] = r / a[INDUT(piv,piv)];
		}
	}
	if (invflag) free(s);
}





void RotationMatrix(PhoParamType *Pphoto)
{
	/* Compute trig functions */
	Pphoto->so = sin(Pphoto->omega);
	Pphoto->co = cos(Pphoto->omega);
	Pphoto->sp = sin(Pphoto->phi);
	Pphoto->cp = cos(Pphoto->phi);
	Pphoto->sk = sin(Pphoto->kappa);
	Pphoto->ck = cos(Pphoto->kappa);

	/* Compute rotation matrix elements */
	Pphoto->m11 = Pphoto->cp * Pphoto->ck;
	Pphoto->m12 = Pphoto->so * Pphoto->sp * Pphoto->ck + Pphoto->co * Pphoto->sk;
	Pphoto->m13 = -Pphoto->co * Pphoto->sp * Pphoto->ck + Pphoto->so * Pphoto->sk;
	Pphoto->m21 = -Pphoto->cp * Pphoto->sk;
	Pphoto->m22 = -Pphoto->so * Pphoto->sp * Pphoto->sk + Pphoto->co * Pphoto->ck;
	Pphoto->m23 = Pphoto->co * Pphoto->sp * Pphoto->sk + Pphoto->so * Pphoto->ck;
	Pphoto->m31 = Pphoto->sp;
	Pphoto->m32 = -Pphoto->so * Pphoto->cp;
	Pphoto->m33 = Pphoto->co * Pphoto->cp;
}