#include "ninox.h"

// asume that +ve angle means rotate left-to-right, -ve angle
// means rotate right-to-left

// assume that rotation is in x-direction only

#define RAD_TO_DEG 57.2957795130823

static int RotateImage8(struct Image *,int,int,int *,int *,double);
static int RotateImage16(struct Image *,int,int,int *,int *,double);
static int RotateImage24(struct Image *,int,int,int *,int *,double);

static int FindExtent(struct Image *img, int *start_x, int *end_x);
static int FindExtent8(struct Image *img, int *start_x, int *end_x);
static int FindExtent16(struct Image *img, int *start_x, int *end_x);
static int FindExtent24(struct Image *img, int *start_x, int *end_x);

#define MASK_EMPTY	0
#define MASK_VAL 	1
#define MASK_SMOOTH 	2
#define MASK_NEW_VAL 	3

#define VALID_PIXEL(x) ((x)==MASK_VAL || (x)==MASK_SMOOTH)

int
RotateImage(struct Image *img, double angle)
	{
	int i,rval,x_centre,y_centre;
	int *start_x, *end_x;

	if (! FindCentre(img,&x_centre,&y_centre)) {
	   Print("RotateImage: Cannot find centre of image.\n");
	   return 0;
	   }

	// For each row in the image find where the object starts and ends
	start_x = ZeroMalloc(img->height * sizeof(int));
	end_x = ZeroMalloc(img->height * sizeof(int));
	rval = 0;

	if (! FindExtent(img,start_x,end_x)) {
	   Print("RotateImage: Cannot find extent of image.\n");
	   exit(1);
	   }

	switch(img->depth) {
	   case 8:
		rval = RotateImage8(img,x_centre,y_centre,start_x,end_x,angle);
		break;
	   case 16:
		rval = RotateImage16(img,x_centre,y_centre,start_x,end_x,angle);
		break;
	   case 24:
		rval = RotateImage24(img,x_centre,y_centre,start_x,end_x,angle);
		break;
	   default:
		Print("RotateImage: depth %d not supported\n",img->depth);
		break;
	   }

	free(start_x); free(end_x);
	return rval;
	}

static int
RotateImage8(struct Image *img, int x_centre,int y_centre,
		int *start_x,int *end_x,
		double angle)
	{
	int i,x,y,x1,fixed;
	int height = img->height;
	int width = img->width;
	int npix = width * height;
	unsigned char *iptr;
	unsigned char *data,*nptr;
	unsigned char *mask,*mptr;  // track holes in output image

	// This is the new data segment for this image
	data = (unsigned char *)ZeroMalloc(npix * sizeof(unsigned char));
	mask = (unsigned char *)ZeroMalloc(npix * sizeof(unsigned char));

	for(y=0; y<height; ++y) {
	   double radius = (end_x[y] - start_x[y] + 1) / 2;
	   double d,a;

	   if (radius>1 && start_x[y]) {
	      iptr=(unsigned char *)img->data + y*width;
	      nptr=data + y*width;
	      mptr=mask + y*width;
	      for(x=start_x[y]; x<=end_x[y]; ++x) {
		d = (double)(x - x_centre) / radius;
		a = asin(d) * RAD_TO_DEG;
		a += angle;

		if (a>-90 && a<90) {
		   double d1 = sin(a/RAD_TO_DEG) * radius;
		   x1 = (int)(d1+0.5) + x_centre;
		   }
		else x1 = -1;

		if (x1>=0) { nptr[x1] = iptr[x]; mptr[x1]=1;}
		}
	     }
	  }

	free(img->data);
	img->data = (void *)data;

	again:
	fixed=0;

	// Fill in holes by averaging
	for(y=0; y<height; ++y) {
	   mptr = mask + y*width;
	   nptr = data + y*width;
	   if (start_x[y] && end_x[y])
		for(x=start_x[y]; x<=end_x[y]; ++x)
		   if (! mptr[x]) {
			int count=0;
			int total=0;
			if (mptr[x-1]) {total += nptr[x-1]; ++count;}
			if (mptr[x+1]) {total += nptr[x+1]; ++count;}
			if (mptr[x-width-1]) {total += nptr[x-width-1]; ++count;}
			if (mptr[x-width]) {total += nptr[x-width]; ++count;}
			if (mptr[x-width+1]) {total += nptr[x-width+1]; ++count;}
			if (mptr[x+width-1]) {total += nptr[x+width-1]; ++count;}
			if (mptr[x+width]) {total += nptr[x+width]; ++count;}
			if (mptr[x+width+1]) {total += nptr[x+width+1]; ++count;}

			if (count) {nptr[x] = total/count; mptr[x]=1; ++fixed;}
			}
	   }
		
	// keep iterating until all pixels are filled in
	if (fixed) goto again;

	free(mask);
	return(1);
	}

static int
RotateImage16(struct Image *img, int x_centre,int y_centre,
		int *start_x,int *end_x,
		double angle)
	{
	int i,x,y,x1,fixed;
	int height = img->height;
	int width = img->width;
	int npix = width * height;
	unsigned short *iptr;
	unsigned short *data,*nptr;
	unsigned short *mask,*mptr;  // track holes in output image

	// This is the new data segment for this image
	data = (unsigned short *)ZeroMalloc(npix * sizeof(unsigned short));
	mask = (unsigned short *)ZeroMalloc(npix * sizeof(unsigned short));

	for(y=0; y<height; ++y) {
	   double radius = (end_x[y] - start_x[y] + 1) / 2;
	   double d,a,a1,R1,R2;

	   if (radius>1 && start_x[y]) {
	      iptr=(unsigned short *)img->data + y*width;
	      nptr=data + y*width;
	      mptr=mask + y*width;
	      R1 = -RotateCutoff; // + 1 - d_Random() * 3; if (R1<-90) R1=-90;
	      R2 = RotateCutoff; // + 1 - d_Random() * 3; if (R1>90) R1=90;
	      for(i=0,x=start_x[y]; x<=end_x[y]; ++x) {
		int mask_val = MASK_VAL;

		d = (double)(x - x_centre) / radius;
		a = asin(d) * RAD_TO_DEG;
		a1 = a + angle;

		if (a1>-90 && a1<90) {
		   double d1 = sin(a1/RAD_TO_DEG) * radius;
		   double dim = 1.0;
		   unsigned short val = iptr[x];

		   x1 = (int)(d1+0.5) + x_centre;

		   if (a1<R1) {
			dim = (90 + a1) / (90 + R1);
		   	mask_val = MASK_SMOOTH; // needs to be smoothed
			}
		   else if (a1>=R2) {
			dim = (90 - a1) / (90 - R2 - 5);
			if (dim<0) dim=0; if (dim>1) dim=1;
		   	mask_val = MASK_SMOOTH; // needs to be smoothed
			}

		   if (mptr[x1]) {
			nptr[x1] /=2; nptr[x1] += (val*dim)/2;
			}
		   else { nptr[x1] = val * dim; }
		   mptr[x1] = mask_val;
		   }
		}
	     }
	  }

	// Fill in holes by averaging
	memcpy(img->data,data,npix * sizeof(unsigned short));
	again:

	for(y=fixed=0; y<height; ++y) {
	   mptr = mask + y*width;
	   iptr = (unsigned short *)img->data + y*width;
	   nptr = data + y*width;
	   if (start_x[y] && end_x[y])
		for(x=start_x[y]-1; x<=end_x[y]+1; ++x)
		   if (mptr[x] == MASK_EMPTY || mptr[x]==MASK_SMOOTH) {
			int count=0;
			int total=0;
			int debug=0;

			if (VALID_PIXEL(mptr[x-1])) 	     {total += iptr[x-1]; ++count;}
			if (VALID_PIXEL(mptr[x])) 	     {total += iptr[x]; ++count;}
			if (VALID_PIXEL(mptr[x+1])) 	     {total += iptr[x+1]; ++count;}
			if (VALID_PIXEL(mptr[x-width-1])) {total += iptr[x-width-1]; ++count;}
			if (VALID_PIXEL(mptr[x-width])) {total += iptr[x-width]; ++count;}
			if (VALID_PIXEL(mptr[x-width+1])) {total += iptr[x-width+1]; ++count;}
			if (VALID_PIXEL(mptr[x+width-1])) {total += iptr[x+width-1]; ++count;}
			if (VALID_PIXEL(mptr[x+width])) {total += iptr[x+width]; ++count;}
			if (VALID_PIXEL(mptr[x+width+1])) {total += iptr[x+width+1]; ++count;}

			if (count) {
				nptr[x] = total/count;
				mptr[x]=MASK_NEW_VAL;
				++fixed;
				}
			}
	   }
	// keep iterating until all pixels are filled in
	memcpy(img->data,data,npix * sizeof(unsigned short));
	if (fixed) {
		// update the mask to activate the new pixels
		for(i=0; i<npix; ++i) if (mask[i] == MASK_NEW_VAL) mask[i] = MASK_VAL;
		goto again;
		}
		
	free(data);
	free(mask);
	return(1);
	}

static int
RotateImage24(struct Image *img, int x_centre,int y_centre,
		int *start_x,int *end_x,
		double angle)
	{
	int i,x,y,x1,fixed;
	int height = img->height;
	int width = img->width;
	int npix = width * height;
	unsigned char *iptr;
	unsigned char *data,*nptr;
	unsigned char *mask,*mptr;  // track holes in output image

	// This is the new data segment for this image
	data = (unsigned char *)ZeroMalloc(npix * sizeof(unsigned char) * 3);
	mask = (unsigned char *)ZeroMalloc(npix * sizeof(unsigned char));

	for(y=0; y<height; ++y) {
	   double radius = (end_x[y] - start_x[y] + 1) / 2;
	   double d,a;

	   if (radius>1 && start_x[y]) {
	      iptr=(unsigned char *)img->data + y*width * 3;
	      nptr=data + y*width * 3;
	      mptr=mask + y*width;
	      for(x=start_x[y]; x<=end_x[y]; ++x) {
		d = (double)(x - x_centre) / radius;
		a = asin(d) * RAD_TO_DEG;
		a += angle;

		if (a>-90 && a<90) {
		   double d1 = sin(a/RAD_TO_DEG) * radius;
		   x1 = (int)(d1+0.5) + x_centre;
		   }
		else x1 = -1;

		if (x1>=0) {
		   nptr[x1*3] = iptr[x*3];
		   nptr[x1*3+1] = iptr[x*3+1];
		   nptr[x1*3+2] = iptr[x*3+2];
		   mptr[x1]=1;
		   }
		}
	     }
	  }

	free(img->data);
	img->data = (void *)data;

	again:
	fixed=0;

	// Fill in holes by averaging
	for(y=0; y<height; ++y) {
	   mptr = mask + y*width;
	   nptr = data + y*width * 3;
	   if (start_x[y] && end_x[y])
		for(x=start_x[y]; x<=end_x[y]; ++x)
		   if (! mptr[x]) {
			int count=0;
			int R=0,G=0,B=0;

			if (mptr[x-1]) {
			   int p=(x-1)*3;
			   B += nptr[p]; G += nptr[p+1]; R += nptr[p+2];
			   ++count;
			   }

			if (mptr[x+1]) {
			   int p=(x+1)*3;
			   B += nptr[p]; G += nptr[p+1]; R += nptr[p+2];
			   ++count;
			   }

			if (mptr[x-width-1]) {
			   int p=(x-width-1)*3;
			   B += nptr[p]; G += nptr[p+1]; R += nptr[p+2];
			   ++count;
			   }

			if (mptr[x-width]) {
			   int p=(x-width)*3;
			   B += nptr[p]; G += nptr[p+1]; R += nptr[p+2];
			   ++count;
			   }

			if (mptr[x-width+1]) {
			   int p=(x-width+1)*3;
			   B += nptr[p]; G += nptr[p+1]; R += nptr[p+2];
			   ++count;
			   }

			if (mptr[x+width-1]) {
			   int p=(x+width-1)*3;
			   B += nptr[p]; G += nptr[p+1]; R += nptr[p+2];
			   ++count;
			   }

			if (mptr[x+width]) {
			   int p=(x+width)*3;
			   B += nptr[p]; G += nptr[p+1]; R += nptr[p+2];
			   ++count;
			   }

			if (mptr[x+width+1]) {
			   int p=(x+width+1)*3;
			   B += nptr[p]; G += nptr[p+1]; R += nptr[p+2];
			   ++count;
			   }

			if (count) {
			   nptr[x*3] =   B/count;
			   nptr[x*3+1] = G/count;
			   nptr[x*3+2] = R/count;
			   mptr[x]=1; ++fixed;
			   }
			}
	   }
		
	// keep iterating until all pixels are filled in
	if (fixed) goto again;

	free(mask);
	return(1);
	}

static int
FindExtent(struct Image *img, int *start_x, int *end_x)
	{
	switch(img->depth) {
	   case 8:
		return FindExtent8(img,start_x,end_x);
		break;
	   case 16:
		return FindExtent16(img,start_x,end_x);
		break;
	   case 24:
		return FindExtent24(img,start_x,end_x);
		break;
	   default:
		Print("FindExtent: depth %d not supported\n",img->depth);
		break;
	   }

	return 0;
	}

static int
FindExtent8(struct Image *img, int *start_x, int *end_x)
	{
	int x,y,count;
	int width = img->width;
	int height = img->height;
	unsigned char *ptr;
	int threshhold = ThreshHold;  // 16 bit data
	
	for(y=count=0; y<height; ++y) {
	   start_x[y] = end_x[y] = 0;
	   ptr = (unsigned char *)img->data + y * width;

	   // find the image start on this row
	   for(x=1; x<width-1; ++x) {
		if (ptr[x-1]>=threshhold && ptr[x]>=threshhold && ptr[x+1]>=threshhold) {
		   start_x[y] = x-1;
		   break;
		   }
		}

	   // find the image end on this row
	   for(x=width-2; x>0; --x)
		if (ptr[x-1]>=threshhold && ptr[x]>=threshhold && ptr[x+1]>=threshhold) {
		   end_x[y] = x+1;
		   break;
		   }

	   if (start_x[y] && end_x[y]) ++count;
	   }
	
	return count;
	}

static int
FindExtent16(struct Image *img, int *start_x, int *end_x)
	{
	int x,y,count;
	int width = img->width;
	int height = img->height;
	unsigned short *ptr;
	int threshhold = ThreshHold * 256;  // 16 bit data
	
	for(y=count=0; y<height; ++y) {
	   start_x[y] = end_x[y] = 0;
	   ptr = (unsigned short *)img->data + y * width;

	   // find the image start on this row
	   for(x=1; x<width-1; ++x) {
		if (ptr[x-1]>=threshhold && ptr[x]>=threshhold && ptr[x+1]>=threshhold) {
		   start_x[y] = x-1;
		   break;
		   }
		}

	   // find the image end on this row
	   for(x=width-2; x>0; --x)
		if (ptr[x-1]>=threshhold && ptr[x]>=threshhold && ptr[x+1]>=threshhold) {
		   end_x[y] = x+1;
		   break;
		   }

	   if (start_x[y] && end_x[y]) ++count;
	   }
	
	return count;
	}

static int
FindExtent24(struct Image *img, int *start_x, int *end_x)
	{
	int x,y,count;
	int width = img->width;
	int height = img->height;
	unsigned char *ptr;
	int threshhold = ThreshHold;  // 24 bit data
	
	for(y=count=0; y<height; ++y) {
	   start_x[y] = 0;
	   ptr = (unsigned char *)img->data + y * width * 3;

	   // find the image start on this row
	   for(x=5; x<width-5; ++x) {
		int o = x * 3;
		double L1 = ptr[o-3] * 0.114 + 
			    ptr[o-2] * 0.587 + 
			    ptr[o-1] * 0.299;

		if (L1>=threshhold) {
		   double L2 = ptr[o+0] * 0.114 + 
			    ptr[o+1] * 0.587 + 
			    ptr[o+2] * 0.299;

		   double L3 = ptr[o+3] * 0.114 + 
			    ptr[o+4] * 0.587 + 
			    ptr[o+5] * 0.299;

		   if (L2>=threshhold && L3>=threshhold) {
		      start_x[y] = x-1;
		      break;
		      }
		   }
		}

	   // find the image end on this row
	   end_x[y] = start_x[y];
	   for(x=width-5; x>start_x[y]; --x) {
		int o = x * 3;
		double L1 = (double)ptr[o-3] * 0.114 + 
			    (double)ptr[o-2] * 0.587 + 
			    (double)ptr[o-1] * 0.299;

		double L2 = (double)ptr[o+0] * 0.114 + 
			    (double)ptr[o+1] * 0.587 + 
			    (double)ptr[o+2] * 0.299;

		double L3 = (double)ptr[o+3] * 0.114 + 
			    (double)ptr[o+4] * 0.587 + 
			    (double)ptr[o+5] * 0.299;

		if (L1>=threshhold && L2>=threshhold && L3>=threshhold) {
		   end_x[y] = x+1;
		   break;
		   }
		}

	   if (start_x[y] && end_x[y]) ++count;
	   }
	
	return count;
	}