/*********************************************************************
  Written by Travis Burkitt 1989-1990
  Program to compute angle/relative of full/normal/component
  velocity fields given the correct flow.
*********************************************************************/
#include        <fcntl.h>
#include        <stdio.h>
#include        <math.h>

#define NUMBINS 100
#define PI M_PI
#define ANGLE 3
#define RELATIVE 4
#define PRINT 0
#define TRUE 1
#define FALSE 0
#define FULL 101
#define NORMAL 102
#define COMPONENT 103
#define PIC_X 300
#define PIC_Y 300
#define UNDEFINED 100000.0
typedef float float3[3];

float getval(),norm();


int 	count = 0;
int	ERROR_TYPE,VELOCITY_TYPE,SINUSOIDAL;
int	bin[NUMBINS+1];
float 	min = 100000.0,max = -100000.0;
float   binval[NUMBINS];
int   	littlebin[3];
float 	littlebinval[3],x_vel_value,y_vel_value;
int 	size,PLOT_GRAPH;
int 	sample = 4;
int 	total1,total2;
double	avg = 0.0;
float 	standard_deviation,term,sum;
float 	differences[22*PIC_X*PIC_Y];
float 	PsiEN(),PsiER(),RelErr();
	
/************************************************************/
main(argc, argv)
int argc;
char **argv;
{
char inname[100],aname[100],outname[100],difference = 0;
int fp1,fp2,fp,i,j,inc,n;
float lower,upper,x,y;
int totx,toty,numx,numy,offx,offy,totx1,toty1,numx1,numy1,offx1,offy1;
float val,uva[2],uve[2];


fprintf(stderr,"\nProgram: %s\n\n",argv[0]);
if (!(argc >= 5 && argc <= 12))
	{
        fprintf(stderr,"Usage:  %s <-A or -R> [-B <lower bound> <upper bound> <number of increments>]  <test-field>  [<-F or -N or -C> <correct-field>] [<-SF or -SN or -SC> <x-value> <y-value>] [-D <diff-field>] \n", argv[0]);
	fprintf(stderr,"Options cannot be re-ordered\n");
	fprintf(stderr,"-A  angle error\n");
	fprintf(stderr,"-R  relative error\n");
	fprintf(stderr,"    one of -A or -R must be present\n");
	fprintf(stderr,"-B  <float> <float> <int> specify lower and upper\n");
	fprintf(stderr,"    bounds for the graph plus an integer for the\n");
	fprintf(stderr,"    number of intervals\n");
	fprintf(stderr,"    if -B is not present graph data is no produced\n");
	fprintf(stderr,"<test-field> computed velocity field>\n");
	fprintf(stderr,"-F  <correct-field> full velocity error\n");
	fprintf(stderr,"-N  <correct-field> normal velocity error\n");
	fprintf(stderr,"-C  <correct-field> component velocity error\n");
	fprintf(stderr,"-SF <float> <float> full error for constant velocity\n");
	fprintf(stderr,"-SN <float> <float> normal error for constant velocity\n");
	fprintf(stderr,"    not compatible with -R\n");
	fprintf(stderr,"-SC <float> <float> component error for constant velocity\n");
	fprintf(stderr,"    not compatible with -R\n");
	fprintf(stderr,"-D  <diff-field> produce difference flow field (for full velocities only)\n");
       	return;
    	}

if(strcmp(argv[1],"-A")==0) ERROR_TYPE = ANGLE;
else if(strcmp(argv[1],"-R")==0) ERROR_TYPE = RELATIVE;
else { fprintf(stderr,"\nFatal error: -A or -R not specified\n"); exit(1); }

if(ERROR_TYPE == ANGLE) { min = 180.0; max = -180.0; }
else { min = HUGE; max = -HUGE; }
SINUSOIDAL = FALSE;
PLOT_GRAPH = FALSE;
i = 2;
if(strcmp(argv[2],"-B")==0)
	{
	PLOT_GRAPH = TRUE;
	sscanf(argv[3],"%f",&lower);
	sscanf(argv[4],"%f",&upper);
	sscanf(argv[5],"%d",&inc);
	if(lower >= upper)
		{
		fprintf(stderr,"Fatal error: lower bound of %f >= upper bound of %f\n",lower,upper);
		exit(1);
		}
	if(inc <= 0)
		{
		fprintf(stderr,"Fatal error: number of increments must be > 0\n");
		exit(1);
		}
	i = 6;
	}

/* Read computed velocity field name */
sprintf(inname,"%s",argv[i]);
i+=1;
if(strcmp(argv[i],"-F")==0 || strcmp(argv[i],"-N")==0 || 
   strcmp(argv[i],"-C")==0)
{
sprintf(aname,"%s",argv[i+1]);
if(strcmp(argv[i],"-F")==0) VELOCITY_TYPE = FULL;
else if(strcmp(argv[i],"-N")==0) VELOCITY_TYPE = NORMAL;
else VELOCITY_TYPE = COMPONENT;
i+=2;
}
else if(strcmp(argv[i],"-SF")==0 || strcmp(argv[i],"-SN")==0 ||
	strcmp(argv[i],"-SC")==0)
{
sscanf(argv[i+1],"%f",&x_vel_value);
sscanf(argv[i+2],"%f",&y_vel_value);
SINUSOIDAL = TRUE;
if(strcmp(argv[i],"-SF")==0) VELOCITY_TYPE = FULL;
else if(strcmp(argv[i],"-SN")==0) VELOCITY_TYPE = NORMAL;
else VELOCITY_TYPE = COMPONENT;
i = i+3;
}
else
{
fprintf(stderr,"\nFatal error: incorrect options - see usage\n");
exit(1);
}
if(i < argc)
if(strcmp(argv[i],"-D")==0) 
	{
    	sprintf(outname,"%s",argv[i+1]);
	if(VELOCITY_TYPE!=FULL)
		{
		printf("Fatal error: velocity type not full - difference field undefined\n");
		exit(1);
		}
	difference = 1;
	if((fp=open(outname,O_WRONLY | O_CREAT,0x01C0))<0)
		{
       		fprintf(stderr,"Error creating difference file %s\n",outname);
       		exit(1);
       		}
	}
if(PRINT)
for(i=0;i<argc;i++)
	{
	printf("i=%d %s\n",i,argv[i]);
	}

if(VELOCITY_TYPE == FULL) fprintf(stderr,"Full velocity error calculation\n");
if(VELOCITY_TYPE == NORMAL) fprintf(stderr,"Normal velocity error calculation\n");
if(VELOCITY_TYPE == COMPONENT) fprintf(stderr,"Component velocity error calculation\n");

if(ERROR_TYPE == ANGLE && PLOT_GRAPH) fprintf(stderr,"Angle error plotted\n");
else if(PLOT_GRAPH) fprintf(stderr,"Relative error plotted\n");
if(sample*inc >= NUMBINS-1) 
	{ 
	fprintf(stderr,"\nFatal error: not enough bins\n");
	exit(1); 
	}
fflush(stdout);

 
if((fp1 = open(inname,O_RDONLY)) < 0 )
	{
        fprintf(stderr,"Error opening test file %s\n",inname);
        exit(1);
        }

if(SINUSOIDAL) fprintf(stderr,"Sinusoidal Input\n");
else fprintf(stderr,"Flow Field Input\n");
if(!SINUSOIDAL)
{
if((fp2 = open(aname,O_RDONLY)) < 0 )
	{
        fprintf(stderr,"Error opening correct file %s\n",aname);
        exit(1);
        }
}

fflush(stdout);

if(PLOT_GRAPH) initbins(lower,upper,inc,ERROR_TYPE);

totx1 = (int)getval(fp1,-1,-1);
toty1 = (int)getval(fp1,-1,-2);
numx1 = (int)getval(fp1,-1,-1);
numy1 = (int)getval(fp1,-1,-2);
offx1 = (int)getval(fp1,-1,-1);
offy1 = (int)getval(fp1,-1,-2);
size = numx1*numy1;
fflush(stdout);

if(VELOCITY_TYPE!=COMPONENT)
	{
	if(!SINUSOIDAL)
	{
	totx = (int)getval(fp2,-1,-1);
        toty = (int)getval(fp2,-1,-2);
        numx = (int)getval(fp2,-1,-1);
        numy = (int)getval(fp2,-1,-2);
        offx = (int)getval(fp2,-1,-1);
        offy = (int)getval(fp2,-1,-2);
	}

if(difference)
	{
	val = totx1; 	write(fp,&val,4);
	val = toty1; 	write(fp,&val,4);
	val = numx1; 	write(fp,&val,4);
	val = numy1; 	write(fp,&val,4);
	val = offx1; 	write(fp,&val,4);
	val = offy1; 	write(fp,&val,4);
    	}

if(!SINUSOIDAL && VELOCITY_TYPE!=COMPONENT)
	if (totx1 != totx || toty1 != toty) 
		{
		fprintf(stderr,"Incompatible files.\n");
		exit(1);	
		}
	
if(!SINUSOIDAL && VELOCITY_TYPE!=COMPONENT)
 	for (j=0;j<offy1;j++) 
		for(i=0;i<numx; i++) 
			{
        		uva[0] = getval(fp2,i,j);
		        uva[1] = getval(fp2,i,j);
			}

	for(i=0;i<numy1;i++)	 
		{
		if(!SINUSOIDAL && VELOCITY_TYPE!=COMPONENT)
 		for (j=0;j<offx1;j++) 
			{
        		uva[0] = getval(fp2,i,j);
		        uva[0] = getval(fp2,i,j);
			}

		for (j=0; j<numx1; j++)	 
			{
        		uve[0] = getval(fp1,i,j);
		        uve[1] = getval(fp1,i,j);
			if(SINUSOIDAL && VELOCITY_TYPE != COMPONENT)
			  {
			  uva[0] = x_vel_value;
			  uva[1] = y_vel_value;
			  }
			else
			  {
        		  uva[0]= getval(fp2,i,j);
		          uva[1]= getval(fp2,i,j);
			  }
			if(uve[0] == 100.0 && uve[1] == 100.0) 
				{
			    	if(difference) 
					{
					write(fp,&uve[0],4);
					write(fp,&uve[1],4);
			    		}
				}
			else 
			{ /* Fixed by adding a set of braces, Nov 10, 1995 */
			errcompute(uva,uve,inc,ERROR_TYPE);

		    	if (difference) 
				{
				val = uve[0]-uva[0];
				write(fp,&val,4);
				val = uve[1]-uva[1];
				write(fp,&val,4);
				}
			}	
			}
		if(!SINUSOIDAL && VELOCITY_TYPE!=COMPONENT)
 		for (j=0;j<(totx1-numx1-offx1);j++) 
			{
        		uva[0] = getval(fp2,i,j);
		        uva[1] = getval(fp2,i,j);
			}
		}
	}
	else /* Component image velocity, i.e. Fleet and Jepson */
	{
	total1 = total2 = 0;
	if(!SINUSOIDAL)
	{
	totx = (int)getval(fp2,-1,-1);
        toty = (int)getval(fp2,-1,-2);
        numx = (int)getval(fp2,-1,-1);
        numy = (int)getval(fp2,-1,-2);
        offx = (int)getval(fp2,-1,-1);
        offy = (int)getval(fp2,-1,-2);
	}
	fflush(stdout);


	/* total1: number of pixels with one or more component velocities */
	/* total2: total number of component velocities */
	n = read(fp1,&x,4);
        while (n==4) 
		{
                read(fp1,&y,4);

                read(fp1,&uve[0],4);
		if(uve[0] != 1000000.0) total1++;
                while (uve[0] != 1000000.0) 
			{
			int pos;

			total2++;
                        read(fp1,&uve[1],4);
			pos = (int)(y*numx+x)*8+24;
			if(!SINUSOIDAL)
			{
 			if (lseek(fp2,pos,0) != pos) 
				{
				printf("Fatal Error: can't  seek\n");	
				exit(1);
				}
		
			read(fp2,&uva[0],4);
			read(fp2,&uva[1],4);
			}
			else
			{
			uva[0] = x_vel_value;
			uva[1] = y_vel_value;
			}
			errcompute(uva,uve,inc,ERROR_TYPE);
                        read(fp1,&uve[0],4);
                	}
                n = read(fp1,&x,4);
        	}
	}
print_tex(lower,upper,inc,ERROR_TYPE,VELOCITY_TYPE);
}


/************************************************************/
initbins(lower,upper,inc,ERROR_TYPE)
float lower,upper;
int inc,ERROR_TYPE;
{
int i;
float err,step;

if(inc != 0)
step = (upper - lower)/(inc*1.0*sample);
else 
{
fprintf(stderr,"\nFatal error: 0 bins specified\n");
exit(1);
}

err = lower;
for(i=0;i<inc*sample;i++)
	{
	binval[i] = err;
	err = err+step;
	}
for (i=0;i<=NUMBINS;i++) bin[i] = 0;
fflush(stdout);
littlebinval[0] = 1.0;
littlebinval[1] = 2.0;
littlebinval[2] = 3.0;
littlebin[0] = littlebin[1] = littlebin[2] = 0.0;
}


/************************************************************/
errcompute(uva,uve,inc,ERROR_TYPE)
float uva[2],uve[2];
int inc,ERROR_TYPE;
{

int i;
float err_val;

if(ERROR_TYPE == RELATIVE) err_val = RelErr(uve,uva);	
else if(VELOCITY_TYPE==FULL) err_val = fabs(PsiER(uve,uva));
else if(VELOCITY_TYPE==NORMAL || VELOCITY_TYPE==COMPONENT)
	err_val = PsiEN(uve,uva);
differences[count] = err_val;

if(err_val!=UNDEFINED)
{
count++;

if(PLOT_GRAPH)
{
for(i=0;i<inc*sample;i++) 
	{
	if(err_val <= binval[i]) bin[i]++;
	}
if(err_val > binval[inc*sample-1]) bin[inc*sample]++;
for(i=0;i<3;i++)
	{
	if(fabs(err_val) <= littlebinval[i]) littlebin[i]++;
	}
}


if(ERROR_TYPE == RELATIVE)
{
if(err_val > 1000.0) err_val = 1000.0;  /* avoid adding in infinity */
avg += err_val;
}

if(ERROR_TYPE == ANGLE)
{
if(fabs(err_val)>=0.0 && fabs(err_val) <= 180.0) avg += err_val;
else 	
	{ 
	fprintf(stderr,"\nFatal error: angle error not within 180 degrees\n"); 
        fprintf(stderr,"Error computed: %f\n",err_val); 
        fprintf(stderr,"Correct velocity: %f,%f Computed velocity: %f,%f\n",
	 	uva[0],uva[1],uve[0],uve[1]); 
	fflush(stderr);
	exit(1);
	}
}	
if(err_val < min) min = err_val;
if(err_val > max) max = err_val;
}
}



/************************************************************/
print_tex(lower_x,upper_x,inc_x,ERROR_TYPE,VELOCITY_TYPE)
float lower_x,upper_x;
int inc_x,ERROR_TYPE,VELOCITY_TYPE;
{
int i,max1,inc_y,int_val;
float max_y;
float lower_y,upper_y;

avg=(avg*1.0)/count;
if(PLOT_GRAPH)
{
printf("\n%f\n",lower_x);
printf("%f\n",upper_x);
printf("%d\n\n",inc_x);
max1 = bin[0];
for(i=1;i<inc_x*sample;i++)
	{
	if((bin[i]-bin[i-1]) > max1) max1 = (bin[i]-bin[i-1]);
	}
if(max1 < bin[inc_x*sample]) max1 = bin[inc_x*sample];
max_y = max1/1000.0;

lower_y = 0.0;
upper_y = (float) (int_val=1.2*max_y*100.0+0.5)/100;
if(upper_y < 0.5) inc_y = 3;
else if(upper_y <=0.8) inc_y = 4;
else if(upper_y <=1.0) inc_y = 5;
else inc_y = 6;
printf("%f\n",lower_y);
printf("%f\n",upper_y);
printf("%d\n\n",inc_y);
if(ERROR_TYPE == ANGLE) printf("%d\n",0); else printf("%d",1);
printf("%d\n",VELOCITY_TYPE);
printf("%f\n",(float) ((bin[0])*1.0/max1)*100.0);
for(i=1;i<inc_x*sample;i++)
	printf("%f\n",(float) ((bin[i]-bin[i-1])*1.0/max1)*100.0);
printf("%f\n",(float) ((bin[inc_x*sample])*1.0/max1)*100.0);
printf("%f\n",(littlebin[0]*1.0)/(count*1.0)*100.0);
printf("%f\n",(littlebin[1]*1.0)/(count*1.0)*100.0);
printf("%f\n",(littlebin[2]*1.0)/(count*1.0)*100.0);
if(VELOCITY_TYPE != COMPONENT)
printf("%f\n",(count*1.0)/(size*1.0)*100.0);
else printf("%d\n",count);
printf("%f\n",avg);
}
sum = 0.0;
for(i=0;i<count;i++)
	{
	term = (avg-differences[i]);
	sum =  term*term + sum;
	}
standard_deviation = sqrt(sum/((count-1)*1.0));
if(PLOT_GRAPH) printf("%f\n",standard_deviation);
fprintf(stderr,"Average Error: %f\n",avg);
fprintf(stderr,"Standard Deviation: %f\n",standard_deviation);
if(VELOCITY_TYPE != COMPONENT)
{
fprintf(stderr,"Count %d\n",count);
fprintf(stderr,"Density %f\n",(count*1.0)/(size*1.0)*100.0);
}
else
{
fprintf(stderr,"Number of Component Velocities %d\n",count);
fprintf(stderr,"Total Potential Number of Component Velocities %d\n",22*size);
fprintf(stderr,"Overall Density %f\n",(count*1.0)/(22.0*size)*100.0);
fprintf(stderr,"Density of Locations with 1 or more Component Velocities: %f\n",
	total1*100.0/size);
fprintf(stderr,"Average Number of Component Velocities for each such Location: %f\n",(total2*1.0)/(total1*1.0));
}
fprintf(stderr,"Min: %f Max: %f\n",min,max);
}

/************************************************************/
float getval(f,i,j)
{
float v;
if (read(f,&v,sizeof(float))!= sizeof(float))
       printf("Read Error: %d %d\n",i,j);
return(v);
}

/************************************************************
   returns the norm of a vector v of length n.
************************************************************/
float norm(v,n)
float v[];
int n;
{
int i;
float sum = 0.0;

for(i=0;i<n; i++)
        sum += (v[i]*v[i]);
sum = sqrt(sum);
return sum;
}

/************************************************************
 Normal Image Velocity Angle Error
************************************************************/
float PsiEN(ve,va)
float3 ve,va;
{
float nva,nve;

float v1,v2;
float3 n;

nva = norm(va,2);
nve = norm(ve,2);

n[0] = ve[0]/nve;
n[1] = ve[1]/nve;
v1 = (va[0]*n[0] + va[1] * n[1] -nve) ;
v2 = v1/(sqrt((1+nva*nva))*sqrt((1+nve*nve)));
v1 =  asin(v2)*180.0/PI;

if(!(v1>=-90.0 && v1<=90.0))
	v1 = UNDEFINED;
return v1;
}


/************************************************************
 Full Image Velocity Angle Error
************************************************************/
float PsiER(ve,va)
float ve[2],va[2];
{
float nva;
float nve;
float v,r,temp;
float VE[3],VA[3];

VE[0] = ve[0];
VE[1] = ve[1];
VE[2] = 1.0;

VA[0] = va[0];
VA[1] = va[1];
VA[2] = 1.0;

nva = norm(VA,3);
nve = norm(VE,3);
v =  (VE[0]*VA[0]+VE[1]*VA[1]+1.0)/(nva*nve);

/**  sometimes roundoff error causes problems **/
if(v > 1.0 && v < 1.0001) v = 1.0;

r = acos(v)*180.0/PI;

if(!(r>=0.0 && r< 180.0))
	{
        printf("ERROR in PSIER()...\n r=%8.4f  v=%8.4f  nva=%8.4f nve= %8.4f\n",r,v,nva,nve);
	printf("va=(%f,%f) ve=(%f,%f)\n",va[0],va[1],ve[0],ve[1]);
	}

return r;
}


/************************************************************
  Compute Relative Error
************************************************************/
float RelErr(ve,va)
float3 ve,va;
{
float3 uv;
uv[0] = ve[0] - va[0];
uv[1] = ve[1] - va[1];
return (norm(uv,2)/norm(va,2)*100.0);
}