#include <sys/types.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include "amssup.h"

/* a transcription of code from pages 210-211 of "Digital Image
 * Warping", by George Wolberg 
 *
 * assume that image is passed in as an array of
 * pixels -- I need to check with graphics guys as
 * to how this is done -- presumably an array of
 * 8-bit values (for 256 colors), so chars
 * 
 * this file will not compile, for obvious reasons
 * 
 * matrix undergoes three transformations, from
 * height * width                 (src)
 * height * maxwidth              (dst)
 * outheight * maxwidth           (tmp)
 * outheight * outwidth           (dst)
 */

#ifdef COLOR
typedef struct pix {
   unsigned char red, green, blue;
} Pixel_Type;
#else
typedef unsigned char Pixel_Type;
#endif

#ifdef COLOR

#define ASSIGN_ZERO(dst) 		\
             dst->red = 0; dst->green = 0; dst->blue = 0
#define ASSIGN_SCALE(dst, src, g)       \
    dst->red = g*src->red; dst->green = g*src->green; dst->blue = g*src->blue
#define INTERPOLATE(dst, src, w1, w2, offset)	\
            dst->red = w1*src[0].red + w2*src[offset].red;\
            dst->green = w1*src[0].green + w2*src[offset].green;\
            dst->blue = w1*src[0].blue + w2*src[offset].blue
#else

#define ASSIGN_ZERO(dst) 		   (*dst = 0)
#define ASSIGN_SCALE(dst,src,g)            (*dst = g * src[0])
#define INTERPOLATE(dst,src,w1,w2,offset)  (*dst = w1*src[0] + w2*src[offset])

#endif

#define HEIGHT 1024
#define WIDTH  1024

#define MAX(a,b) (a<b ? b : a)
#define MIN(a,b) (a<b ? a : b)

#ifdef TILE
#define TILESIZE 32
void tiled_skew_start(Pixel_Type *src, int len, int nlen,
		     double start, int offset, Pixel_Type *dst);
void tiled_skew(Pixel_Type *src, int len, int nlen,
	       double start, int offset, Pixel_Type *dst,
	       int tile_start, int tile_length);
void tiled_skew_end(Pixel_Type *src, int len, int nlen,
		   double start, int offset, Pixel_Type *dst);
#endif

/* declarations */
void indices (Pixel_Type *start, Pixel_Type *current,
	     int *x, int *y, int width);
void rotate (Pixel_Type *image, int height, int width, double angle, 
	    Pixel_Type *outimage, int *outh, int *outw, Pixel_Type *tmpa);
void skew(Pixel_Type *src, int len, int nlen,
	 double start, int offset, Pixel_Type *dst);
void write_pgm(char *name, Pixel_Type *image, int height, int width);

/*********************************************************************/

main()
{
#ifdef COLOR
   char pada[0xed0];  Pixel_Type bars[HEIGHT][WIDTH];
   char padb[0x2000]; Pixel_Type new_bars[2*HEIGHT][2*WIDTH];
   char padc[0x2000]; Pixel_Type tmparray[2*HEIGHT][2*WIDTH];
#else
   Pixel_Type pada[0xec8], bars[HEIGHT][WIDTH];
   Pixel_Type padb[0x1000], new_bars[2*HEIGHT][2*WIDTH];
   Pixel_Type padc[0x1000], tmparray[2*HEIGHT][2*WIDTH];
#endif
   int      i, j;
   int      newheight, newwidth;
   struct  rusage start, end;

   aprintf("Image rotation: Height = %d, Width = %d, %s image\n", 
            HEIGHT, WIDTH, (sizeof(Pixel_Type) == 1) ? "BW" : "Color");
   aprintf("  bars = 0x%x, new_bars = 0x%x, tmp = 0x%x\n", 
	   bars, new_bars, tmparray);
   aprintf("Initializing at cycle: %d ... \n", read_itimer());

#if 0
   for (j = 0; j < HEIGHT; j++) {
      for (i = 0; i < WIDTH; i++) {
	   if ((i % 20) < 10) {
#ifdef COLOR
                bars[j][i].red = 128;
                bars[j][i].green = 0;
                bars[j][i].blue = 0;
#else
                bars[j][i] = 228;
#endif
            } else {
#ifdef COLOR
                bars[j][i].red = 0;
                bars[j][i].green = 0;
                bars[j][i].blue = 128;
#else
                bars[j][i] = 64;
#endif
	   }
       }
   }
#endif
   
#ifdef DEBUG
   write_pgm ("original", &bars[0][0], HEIGHT, WIDTH);
   getrusage(0, &start);
#endif

   rotate (&bars[0][0], HEIGHT, WIDTH, 1, 
	   &new_bars[0][0], &newheight, &newwidth,
	   &tmparray[0][0]);

#ifdef DEBUG
   getrusage(0, &end);
   write_pgm ("new", &new_bars[0][0], newheight, newwidth);

   fprintf (stderr, "user time is %g, system time is %g\n",

	    (end.ru_utime.tv_sec - start.ru_utime.tv_sec)*1.0e6+
	    (end.ru_utime.tv_usec - start.ru_utime.tv_usec),

	    (end.ru_stime.tv_sec - start.ru_stime.tv_sec)*1.0e6+
	    (end.ru_stime.tv_usec - start.ru_stime.tv_usec)
	    );
#endif

  aprintf("End of program at cycle: %d\n", read_itimer());

  return(0);
}

void rotate (Pixel_Type *image,    /* height x width */
	     int width, int height,
             double angle,            /* rotation angle */
	     Pixel_Type *outimage,          /* dimensions are computed below */
	     int *outh, int *outw, Pixel_Type *tmparray)
{
  int x;
  int y;

  double sinT   = sin(angle);
  double cosT   = cos(angle);
  double tanT2  = tan(angle/2.0);
  int maxwidth  = width + height*tanT2 + 1; /* width of intermediate image */
  int outheight = width*sinT + height*cosT + 1; /* height of final image   */
  int outwidth  = height*sinT + width*cosT + 1; /* width of final image    */

  Pixel_Type *tmp = tmparray;

  /* pointers to buffers during shear */
  Pixel_Type *src;
  Pixel_Type *dst;

  /* offset from top of image */
  int offset = (width-1)*sinT;

#ifdef IMPULSE
  Pixel_Type *outimage_transpose;
  Pixel_Type *tmp_transpose;
  Pixel_Type *impulse_remap();
#endif

  assert (0 <= angle && angle <= M_PI_2);

  /* 
   * shear horizontal direction 
   */
  aprintf("Shearing horizontal direction at cycle: %d\n", read_itimer());

  for (y = 0; y < height; y++) {
      src = &image[y*width];        /* beginning of input row  */
      dst = &outimage[y*maxwidth];  /* beginning of output row */

      /* skew each x by -y*tanT2 */
      skew(src, width, maxwidth, y*tanT2, 1, dst);
  }

#ifdef DEBUG
  write_pgm ("tmp1", &outimage[0], height, maxwidth);
#endif

  /* 
   * shear vertical direction 
   */
  aprintf("Shearing vertical direction at cycle: %d\n", read_itimer());

#ifdef IMPULSE
  /* remap matrices outimage and tmp to their transposes */

  tmp_transpose      = impulse_remap(tmp, outheight, maxwidth, 0, 0);
  outimage_transpose = impulse_remap(outimage, height, maxwidth, 8192, 8192);

  /* shear vertical direction */
  for (x = 0; x < maxwidth; x++) {
      src = &outimage_transpose[x*height];
      dst = &tmp_transpose[x*outheight];

      /* skew each x by y*sinT, plus shift image by offset too */
      skew(src, height, outheight, offset-x*sinT, 1, dst);
  }

  /* flush cache of tmp */
  flushcache(tmp_transpose, outheight * maxwidth);

#else

#ifdef TILE /* tiled shear in vertical direction */

  /* handle beginning of scanline */
  for (x = 0; x < maxwidth; x++) {
      src = &outimage[x];
      dst = &tmp[x];

      /* skew each y by x*sinT, plus shift image back down */
      /* skew(src, height, outheight, offset-x*sinT, maxwidth, dst); */
      tiled_skew_start(src, height, outheight, offset-x*sinT, maxwidth, dst);
  }

  /* tile with respect to the accesses to the original scanline */
  for (y = 0; y < height; y += TILESIZE) {
      for (x = 0; x < maxwidth; x++) {
	  src = &outimage[x];
	  dst = &tmp[x];

	  /* skew each y by x*sinT, plus shift image back down */
	  /* skew(src, height, outheight, offset-x*sinT, maxwidth, dst); */

	  tiled_skew(src, height, outheight, offset-x*sinT, maxwidth, dst, y, TILESIZE);
      }
  }

  /* handle end of scanline */
  for (x = 0; x < maxwidth; x++) {
      src = &outimage[x];
      dst = &tmp[x];

      /* skew each y by x*sinT, plus shift image back down */
      /* skew(src, height, outheight, offset-x*sinT, maxwidth, dst); */
      tiled_skew_end(src, height, outheight, offset-x*sinT, maxwidth, dst);
  }

#else /* NON-TILE */
  for (x = 0; x < maxwidth; x++) {
      src = &outimage[x];
      dst = &tmp[x];

      /* skew each y by x*sinT, plus shift image back down */
      skew(src, height, outheight, offset-x*sinT, maxwidth, dst);
  }
#endif /* TILE */

#endif /* IMPULSE */

#ifdef DEBUG
  write_pgm ("tmp2", &tmp[0], outheight, maxwidth);
#endif

  /* 
   * shear horizontal direction 
   */
  aprintf("Skew each y by -x*tanT2 at cycle: %d\n", read_itimer());

  for (y = 0; y < outheight; y++) {
      src = &tmp[y*maxwidth];
      dst = &outimage[y*outwidth];
      
      /* skew each x by -y*tanT2, plus shift image left */
      skew(src, maxwidth, outwidth, (y-offset)*tanT2, 1, dst);
  }

  *outh = outheight;
  *outw = outwidth;
}

/* 
 * skew scanline from src (length len) into dst (length nlen),
 * starting at position strt.  The offset between each scanline
 * pixel is offset.  offset=1 for rows, offset= width for columns
 */
void skew(Pixel_Type *src, int len, int nlen,
	double start, int offset, Pixel_Type *dst)
{
  int i, istart, lim;
  double f, g, w1, w2;

  /* process left of output: either prepare for clipping or add padding */
  istart = (int) start;                /* integer index */
  if (istart < 0) src -= offset*istart;/* advance input pointer for clipping */
  lim = MIN (len+istart, nlen);        /* index for right edge */
  for (i = 0; i < istart; i++) {       /* visit all null output pixels at left edge */
      ASSIGN_ZERO(dst);                /* pad with 0 */
      dst += offset;                   /* advance output pointer */
  }

  f = abs(start-istart);              /* weight for right straddle */
  g = 1.0-f;                          /* weight for left straddle */
  if (f == 0.0) {                     /* simple integer shift: no interpolation */
      for (; i < lim; i++) {          /* visit all pixels in valid range */
	  *dst = *src;                /* copy input to output */
	  src += offset;              /* advance input pointer */
	  dst += offset;              /* advance output pointer */
      }  
  } else {                            /* fractional shift: interpolate */
      if (start > 0.0) {  
	  w1 = f;                     /* weight for left pixel */
	  w2 = g;                     /* weight for right pixel */
	  ASSIGN_SCALE(dst,src,g);    /* first pixel */
	  dst += offset;              /* advance output pointer */
	  i++;                        /* increment index */
      } else {  
	  w1 = g;                     /* weight for left pixel */
	  w2 = f;                     /* weight for right pixel */
	  if (lim < nlen) lim--;  
      }  
      for (; i < lim; i++) {          /* visit all pixels in valid range */
	  /* src[0] is left (top) pixel and 
             src[offset] is right (bottom) pixel */
	  INTERPOLATE(dst,src,w1,w2,offset); /* linear interpolation */
	  dst += offset;              /* advance output pointer */
	  src += offset;              /* advance input pointer */
      }  
      if (i < nlen) {	 
	  ASSIGN_SCALE(dst,src,w1);   /* src[0] is last pixel */
	  dst += offset;              /* advance output pointer */
	  i++;                        /* increment output index */
      }  
  }  
  for (; i < nlen; i++) {             /* visit all remaining pixels at right edge */
      ASSIGN_ZERO(dst);               /* pad with 0 */
      dst += offset;                  /* advance output pointer */
  }
}

void tiled_skew_start(Pixel_Type *src, int len, int nlen,
		    double start, int offset, Pixel_Type *dst)
{
  int i, istart, lim;
  double f, g, w1, w2;

  /* process left of output: either prepare for clipping or add padding */
  istart = (int) start;                /* integer index */

  /* this clip is legal because the rotation steps guarantee that
     there is no part of the image that is getting clipped! */
  if (istart < 0) src -= offset*istart; /* advance input pointer for clipping */

  for (i = 0; i < istart; i++) {  /* visit all null output pixels at left edge */
      ASSIGN_ZERO(dst);           /* pad with 0 */
      dst += offset;              /* advance output pointer */
  }
}


void tiled_skew(Pixel_Type *src, int len, int nlen,
	      double start, int offset, Pixel_Type *dst,
	      int tile_start, int tile_length)
{
  int i, istart, lim;
  double f, g, w1, w2;
  Pixel_Type *original_src = src, *original_dst = dst;

  int end;

  /* process left of output: either prepare for clipping or add padding */
  istart = (int) start;                /* integer index */

  f = abs(start-istart);               /* weight for right straddle */
  g = 1.0-f;                           /* weight for left straddle */

  if (start > 0.0) {
      w1 = f;                          /* weight for left pixel */
      w2 = g;                          /* weight for right pixel */
  } else {
      w1 = g;                          /* weight for left pixel */
      w2 = f;                          /* weight for right pixel */
  }

  /* adjust tile length */
  if (tile_start + tile_length > len) {
      tile_length = len - tile_start;
  }

  lim = MIN (len+istart, nlen);        /* index for right edge */
  end = MIN (tile_start+tile_length - 1, lim);

  /* adjust for clipping */
  if (istart < 0) {
      i = MAX (tile_start + istart, 0);
      end = MIN (MAX (tile_start + istart + tile_length, 0), nlen);
      if (tile_start + istart < 0) {
	  /* short tile */
	  src += offset*(tile_start - (istart % tile_length));
      } else {
	  src += offset*tile_start;
      }
      dst += offset*(MAX(tile_start + istart, 0));
  } else {
      i = tile_start+istart;
      end = MIN (i + tile_length, nlen);
      src += offset*tile_start;
      dst += offset*(tile_start + istart);
  }

  if (f == 0.0) {                      /* simple integer shift: no interpolation */
      for (; i < end; i++) {           /* visit all pixels in valid range */
	  *dst = *src;                 /* copy input to output */
	  src += offset;                       /* advance input pointer */
	  dst += offset;                       /* advance output pointer */
      }
  } else {                             /* fractional shift: interpolate */
      if (start > 0.0 && tile_start == istart) {
	  ASSIGN_SCALE(dst, src, g);   /* first pixel */
	  dst += offset;               /* advance output pointer */
	  i++;                         /* increment index */
      } else {
	  if (lim < nlen) lim--;
      }
      for (; i < end; i++) {           /* visit all pixels in valid range */
	  /* src[0] is left (top) pixel and src[offset] is right (bottom) pixel */
	  INTERPOLATE(dst,src,w1,w2,offset);   /* linear interpolation */
	  dst += offset;                       /* advance output pointer */
	  src += offset;                       /* advance input pointer */
      }
      if (i < nlen) {
	  ASSIGN_SCALE(dst,src,w1);    /* src[0] is last pixel */
	  dst += offset;               /* advance output pointer */
	  i++;                         /* increment output index */
      }
  }
}

void tiled_skew_end(Pixel_Type *src, int len, int nlen,
		  double start, int offset, Pixel_Type *dst)
{
  int i, istart, lim;
  double f, g, w1, w2;

  /* process left of output: either prepare for clipping or add padding */
  istart = (int) start;                /* integer index */
  lim = MIN (len+istart, nlen);        /* index for right edge */

  dst += offset*lim;
  for (i = lim; i < nlen; i++) {/* visit all remaining pixels at right edge */
      ASSIGN_ZERO(dst);                /* pad with 0 */
      dst += offset;                   /* advance output pointer */
  }
}

void write_pgm(char *name, Pixel_Type *image, int height, int width)
{
  char x[128];
  FILE *f;
  int   i;

#ifdef TILE
  sprintf (x, "tiled/%s.pgm", name);
#else
  sprintf (x, "nontiled/%s.pgm", name);
#endif
  f = fopen (x, "w");

#ifdef COLOR
    fprintf(f, "P6\n%d %d\n255\n", width, height);
    for (i = 0; i < width*height; i++) {
        fwrite(&image[i], 3, 1, f);
    }
#else
  fprintf(f, "P5\n%d %d\n255\n", width, height);
  fwrite(image, 1, height*width, f);
#endif

  fclose(f);

  fprintf (stderr, "printing %s.pgm, height=%d, width=%d\n",
	   name, height, width);
}

void indices (Pixel_Type *start, Pixel_Type *current,
	    int *x, int *y, int width)
{
  *x = (current - start) % width;
  *y = (current - start) / width;
}

#ifdef IMPULSE

Pixel_Type *impulse_remap(double *x, int row, int col, int palign, int valign)
{
  Pixel_Type *newp = 0;
  basestride_args_t  bs_args;
  unsigned start;

  aprintf("Calling ams_mapshadow (0x%x, row = %d, col = %d)\n", x, row, col);

  bs_args.p_newaddr      = (unsigned *) &newp;
  bs_args.p_vaddr        = (unsigned) x;
  bs_args.p_count        = row;
  bs_args.p_objsize      = sizeof(Pixel_Type);
  bs_args.stride         = sizeof(Pixel_Type) * col;
  bs_args.offset         = 0;
  bs_args.prefetch_count = 0;
  bs_args.prefetch_info  = 0;
  bs_args.alignment      = palign;
  bs_args.valign         = valign;

  start = read_itimer();
  if (ams_mapshadow(AMS_TYPE_TRANSPOSE, &bs_args) == -1) {
     aprintf("ams_mapshadow failed.\n");
     exit(1);
  }

  aprintf("  ams_mapshadow return 0x%x by using %d cycles\n", 
	  newp, read_itimer()-start);

  return newp;
}

#endif