/* Distcorr:	Performs distortion correction. The input image is
			pointed to by indat. It is corrected using the 
			images delx and dely (the ideal location of each pixel),
			and placed (as a floating point image) in tempbuf.
			The variable size contains the size of the image.

			Each pixel is taken to its ideal location and distributed
			there according to its calculated ideal width and height
			among the nine pixels closest to the point. Any pixel
			of value 32767 is distributed as 32767 to each destination
			pixel. No pixel is allowed to exceed 32767.

Written by Sandor L. Barna, December 1993.
*/



#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include "objects.h"
int distcorr(unsigned short *indat,float *tempbuf,float *delx,float *dely,
 int size, int pedestal)
{
int width,wid1;
int height,hig1;
int length;
int i,j;
int x1,y1;
float xwid,ywid;
float xfract,yfract;
float x,y;
float *xline;
float *yline;
float *yprev;
float *destpix;
float col1,col2,col3,row1,row2,row3;
float pix11,pix12,pix13,pix21,pix22,pix23,pix31,pix32,pix33;
unsigned short *inrow;
unsigned short *inpix;

int low_end, *i_data, saturated;
unsigned short int i2_data[2];

i_data = (int *)&i2_data[0];
*i_data = 1;
if (i2_data[0] == 0)
  low_end = 1;
else
  low_end = 0;
if (pedestal > 0)
  saturated = 65535;
else
  saturated = 32767;
width=size;
wid1=width-1;
height=size;
hig1=height-1;
length=size*size;

memset(tempbuf,0,length*4);

printf("On line: 0 ");
fflush(stdout);

/* Do first point */
xline=delx;
yline=dely;
inrow=indat;
j=0;
x=xline[0];
y=yline[0];
if (x>-1000.0) {
	ywid=yline[width]-y;
	xwid=xline[1]-x;
	if  (ywid<=0.0 || xwid <=0.0 ) {
		xwid=1.0;
		ywid=1.0;
		}
	xwid=1.0/xwid;
	ywid=1.0/ywid;
	x1=(int)(x+0.5);
	y1=(int)(y+0.5);
	xfract=x-x1;
	yfract=y-y1;
	if ((col1=0.5-(xfract+0.5)*xwid)<0.0) col1=0.0;
	if ((col3=0.5+(xfract-0.5)*xwid)<0.0) col3=0.0;
	col2=1.0-col1-col3;
	if ((row1=0.5-(yfract+0.5)*ywid)<0.0) row1=0.0;
	if ((row3=0.5+(yfract-0.5)*ywid)<0.0) row3=0.0;
	row2=1.0-row1-row3;
	inpix=inrow;
        i2_data[low_end] = *inpix;
	pix11=(*i_data)*row1*col1;
	pix12=(*i_data)*row1*col2;
	pix13=(*i_data)*row1*col3;
	pix21=(*i_data)*row2*col1;
	pix22=(*i_data)*row2*col2;
	pix23=(*i_data)*row2*col3;
	pix31=(*i_data)*row3*col1;
	pix32=(*i_data)*row3*col2;
	pix33=(*i_data)*row3*col3;
	destpix=tempbuf+x1-1+(y1-1)*width;
	if((destpix+2*width+2>tempbuf+length)||(destpix<tempbuf)) 
			goto qout;
	if ((*i_data)==saturated) {
		(*destpix)=saturated;		
		destpix++;
		(*destpix)=saturated;
		destpix++;
		(*destpix)=saturated;
		destpix+=width-2;
		(*destpix)=saturated;
		destpix++;
		(*destpix)=saturated;
		destpix++;
		(*destpix)=saturated;
		destpix+=width-2;
		(*destpix)=saturated;
		destpix++;
		(*destpix)=saturated;
		destpix++;
		(*destpix)=saturated;
		}
	else {
		*destpix=min(pix11+*destpix,saturated);		
		destpix++;
		*destpix=min(pix12+*destpix,saturated);		
		destpix++;
		*destpix=min(pix13+*destpix,saturated);		
		destpix+=width-2;
		*destpix=min(pix21+*destpix,saturated);		
		destpix++;
		*destpix=min(pix22+*destpix,saturated);		
		destpix++;
		*destpix=min(pix23+*destpix,saturated);		
		destpix+=width-2;
		*destpix=min(pix31+*destpix,saturated);		
		destpix++;
		*destpix=min(pix32+*destpix,saturated);		
		destpix++;
		*destpix=min(pix33+*destpix,saturated);		
		}
	}

/* Now do first row */
for (j=1;j<width;j++) {
  x=xline[j];
  y=yline[j];
  if (x>-1000.0) {
	ywid=yline[width+j]-y;
	xwid=x-xline[j-1];
	if (ywid<=0.0 || xwid>5.0 ) {
		xwid=1.0;
		ywid=1.0;
		}
	xwid=1.0/xwid;
	ywid=1.0/ywid;
	x1=(int)(x+0.5);
	y1=(int)(y+0.5);
	xfract=x-x1;
	yfract=y-y1;
	if ((col1=0.5-(xfract+0.5)*xwid)<0.0) col1=0.0;
	if ((col3=0.5+(xfract-0.5)*xwid)<0.0) col3=0.0;
	col2=1.0-col1-col3;
	if ((row1=0.5-(yfract+0.5)*ywid)<0.0) row1=0.0;
	if ((row3=0.5+(yfract-0.5)*ywid)<0.0) row3=0.0;
	row2=1.0-row1-row3;
	inpix=inrow+j;
        i2_data[low_end] = *inpix;
	pix11=(*i_data)*row1*col1;
	pix12=(*i_data)*row1*col2;
	pix13=(*i_data)*row1*col3;
	pix21=(*i_data)*row2*col1;
	pix22=(*i_data)*row2*col2;
	pix23=(*i_data)*row2*col3;
	pix31=(*i_data)*row3*col1;
	pix32=(*i_data)*row3*col2;
	pix33=(*i_data)*row3*col3;
	destpix=tempbuf+x1-1+(y1-1)*width;
	if((destpix+2*width+2>tempbuf+length)||(destpix<tempbuf)) 
			goto qout;
	if ((*i_data)==saturated) {
		(*destpix)=saturated;		
		destpix++;
		(*destpix)=saturated;
		destpix++;
		(*destpix)=saturated;
		destpix+=width-2;
		(*destpix)=saturated;
		destpix++;
		(*destpix)=saturated;
		destpix++;
		(*destpix)=saturated;
		destpix+=width-2;
		(*destpix)=saturated;
		destpix++;
		(*destpix)=saturated;
		destpix++;
		(*destpix)=saturated;
		}
	else {
		*destpix=min(pix11+*destpix,saturated);		
		destpix++;
		*destpix=min(pix12+*destpix,saturated);		
		destpix++;
		*destpix=min(pix13+*destpix,saturated);		
		destpix+=width-2;
		*destpix=min(pix21+*destpix,saturated);		
		destpix++;
		*destpix=min(pix22+*destpix,saturated);		
		destpix++;
		*destpix=min(pix23+*destpix,saturated);		
		destpix+=width-2;
		*destpix=min(pix31+*destpix,saturated);		
		destpix++;
		*destpix=min(pix32+*destpix,saturated);		
		destpix++;
		*destpix=min(pix33+*destpix,saturated);		
		}
	}
  }

/* Now do rest */
for (i=1;i<height;i++) {
	xline=delx+i*width;
	yline=dely+i*width;
	yprev=dely+(i-1)*width;
	inrow=indat+i*width;
	/* Do first point */
	j=0;
	x=xline[0];
	y=yline[0];
	if (x>-1000.0) {
		ywid=y-yprev[0];
		xwid=xline[1]-x;
		if (ywid > 5.0 || xwid <= 0.0 ) {
			xwid=1.0;
			ywid=1.0;
			}
		xwid=1.0/xwid;
		ywid=1.0/ywid;
		x1=(int)(x+0.5);
		y1=(int)(y+0.5);
		xfract=x-x1;
		yfract=y-y1;
		if ((col1=0.5-(xfract+0.5)*xwid)<0.0) col1=0.0;
		if ((col3=0.5+(xfract-0.5)*xwid)<0.0) col3=0.0;
		col2=1.0-col1-col3;
		if ((row1=0.5-(yfract+0.5)*ywid)<0.0) row1=0.0;
		if ((row3=0.5+(yfract-0.5)*ywid)<0.0) row3=0.0;
		row2=1.0-row1-row3;
		inpix=inrow;
                i2_data[low_end] = *inpix;
		pix11=(*i_data)*row1*col1;
		pix12=(*i_data)*row1*col2;
		pix13=(*i_data)*row1*col3;
		pix21=(*i_data)*row2*col1;
		pix22=(*i_data)*row2*col2;
		pix23=(*i_data)*row2*col3;
		pix31=(*i_data)*row3*col1;
		pix32=(*i_data)*row3*col2;
		pix33=(*i_data)*row3*col3;
		destpix=tempbuf+x1-1+(y1-1)*width;
		if((destpix+2*width+2>tempbuf+length)||(destpix<tempbuf)) 
				goto qout;
		if ((*i_data)==saturated) {
			(*destpix)=saturated;		
			destpix++;
			(*destpix)=saturated;
			destpix++;
			(*destpix)=saturated;
			destpix+=width-2;
			(*destpix)=saturated;
			destpix++;
			(*destpix)=saturated;
			destpix++;
			(*destpix)=saturated;
			destpix+=width-2;
			(*destpix)=saturated;
			destpix++;
			(*destpix)=saturated;
			destpix++;
			(*destpix)=saturated;
			}
		else {
			*destpix=min(pix11+*destpix,saturated);		
			destpix++;
			*destpix=min(pix12+*destpix,saturated);		
			destpix++;
			*destpix=min(pix13+*destpix,saturated);		
			destpix+=width-2;
			*destpix=min(pix21+*destpix,saturated);		
			destpix++;
			*destpix=min(pix22+*destpix,saturated);		
			destpix++;
			*destpix=min(pix23+*destpix,saturated);		
			destpix+=width-2;
			*destpix=min(pix31+*destpix,saturated);		
			destpix++;
			*destpix=min(pix32+*destpix,saturated);		
			destpix++;
			*destpix=min(pix33+*destpix,saturated);		
			}
		}
	/* Now, rest of the row */
	for (j=1;j<width;j++) {
		x=xline[j];
		y=yline[j];
		if (x>-1000.0) {
			ywid=y-yprev[j];
			xwid=x-xline[j-1];
			if (ywid>5.0 || xwid >5.0 ) {
				xwid=1.0;
				ywid=1.0;
				}
			xwid=1.0/xwid;
			ywid=1.0/ywid;
			x1=(int)(x+0.5);
			y1=(int)(y+0.5);
			xfract=x-x1;
			yfract=y-y1;
			if ((col1=0.5-(xfract+0.5)*xwid)<0.0) col1=0.0;
			if ((col3=0.5+(xfract-0.5)*xwid)<0.0) col3=0.0;
			col2=1.0-col1-col3;
			if ((row1=0.5-(yfract+0.5)*ywid)<0.0) row1=0.0;
			if ((row3=0.5+(yfract-0.5)*ywid)<0.0) row3=0.0;
			row2=1.0-row1-row3;
			inpix=inrow+j;
                        i2_data[low_end] = *inpix;
			pix11=(*i_data)*row1*col1;
			pix12=(*i_data)*row1*col2;
			pix13=(*i_data)*row1*col3;
			pix21=(*i_data)*row2*col1;
			pix22=(*i_data)*row2*col2;
			pix23=(*i_data)*row2*col3;
			pix31=(*i_data)*row3*col1;
			pix32=(*i_data)*row3*col2;
			pix33=(*i_data)*row3*col3;
			destpix=tempbuf+x1-1+(y1-1)*width;
			if((destpix+2*width+2>tempbuf+length)||(destpix<tempbuf)) 
					goto qout;
			if ((*i_data)==saturated) {
				(*destpix)=saturated;		
				destpix++;
				(*destpix)=saturated;		
				destpix++;
				(*destpix)=saturated;
				destpix+=width-2;
				(*destpix)=saturated;
				destpix++;
				(*destpix)=saturated;
				destpix++;
				(*destpix)=saturated;
				destpix+=width-2;
				(*destpix)=saturated;
				destpix++;
				(*destpix)=saturated;
				destpix++;
				(*destpix)=saturated;
				}
			else {
				*destpix=min(pix11+*destpix,saturated);		
				destpix++;
				*destpix=min(pix12+*destpix,saturated);		
				destpix++;
				*destpix=min(pix13+*destpix,saturated);		
				destpix+=width-2;
				*destpix=min(pix21+*destpix,saturated);		
				destpix++;
				*destpix=min(pix22+*destpix,saturated);		
				destpix++;
				*destpix=min(pix23+*destpix,saturated);		
				destpix+=width-2;
				*destpix=min(pix31+*destpix,saturated);		
				destpix++;
				*destpix=min(pix32+*destpix,saturated);		
				destpix++;
				*destpix=min(pix33+*destpix,saturated);		
				}
			}
		}    	
	if (i % 100 ==0) {
		printf("%d ",i);
		fflush(stdout);
		}
	}

printf("\n");
return(0);

qout: printf("\n");
return(1);
}