/* See {dsm_image.h}. */
/* Last edited on 2016-04-10 02:27:12 by stolfilocal */

#define _GNU_SOURCE
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <limits.h>
#include <assert.h>

#include <bool.h>

#include <dsm_image.h>

/* INTERNAL PROTOTYPES */

void dsm_image_read_pnm_header(FILE *rd, int *magic, int *ncols, int *nrows, int *maxval);
void dsm_image_read_pgm_header(FILE *rd, int *magic, int *ncols, int *nrows, int *maxval);
void dsm_image_read_ppm_header(FILE *rd, int *magic, int *ncols, int *nrows, int *maxval);

/* Used to perform morphological operations on an iname
  with {nx} columns and {ny} rows.  Stores into {*nkP} and {(*dkP)[0..nk-1]} the displacements 
  between indices of elements {(x1,y1)} and {(x2,y2)}
  whose Euclidean distance is at most {r}.  Allocates the vector {**dkP}
  if not NULL. otherwise assumes that it has at least {(2*r+1)**2}. */
void dsm_image_compute_disk_displacements(int nx, int ny, int r, int **dkP, int *nkP);

/* IMPLEMENTATIONS */

dsm_image_t dsm_image_alloc (int nx, int ny, uint8_t fill) 
  {
    dsm_image_t img;
    img.nx = nx;
    img.ny = ny;
    img.p = (uint8_t *)malloc(nx*ny * sizeof(uint8_t));
    assert (img.p != NULL);
    for (int k = 0; k < nx*ny; k++) { img.p[k] = fill; }
    return img;
  }


void dsm_image_realloc(dsm_image_t *img, int nx, int ny, uint8_t fill)
  { 
    /* Allocate memory, if necessary, and set pointer */
    if (img == NULL) { 
      /* Allocate image and initialize descriptor. */
      (*img) = dsm_image_alloc(nx, ny, fill);
    } else {
      /* Discard the pixel array if too small: */
      if ((img->p != NULL) && (nx*ny > img->nx*img->ny)) {
	free(img->p); img->p = NULL;
      }
      if (img->p == NULL) {
	/* Reallocate pixel array. */
	img->p = (uint8_t *)malloc(nx*ny * sizeof(uint8_t));
      }
      img->nx = nx;
      img->ny = ny;
      assert (img->p != NULL);
      for (int k = 0; k < nx*ny; k++) { img->p[k] = fill; }
    }
  }

void dsm_image_copy(dsm_image_t *img, dsm_image_t *copy) 
  {
    int nx = img->nx;
    int ny = img->ny;
    assert((nx == copy->nx) && (ny == copy->ny)); 
    for (int y = 0; y < ny; y++) {
      for (int x = 0; x < nx; x++) {
	copy->p[y * nx + x] = img->p[y * nx + x];  
      }   
    }
  }

void dsm_image_crop(dsm_image_t *full, int dx, int dy, dsm_image_t *crop)
  { 
    int nx = crop->nx;
    int ny = crop->ny;
    assert((dx >= 0) && (dx + nx <= full->nx));
    assert((dy >= 0) && (dy + ny <= full->ny));
    for (int y = 0; y < ny; y++) {
      for (int x = 0; x < nx; x++) {
	crop->p[y * crop->nx + x] = full->p[(y + dy) * full->nx + (x + dx)];  
      }   
    }
  }  

void dsm_image_difference(dsm_image_t *img0, dsm_image_t *img1, double scale, dsm_image_t *diff)
  {
    int nx = diff->nx;
    int ny = diff->ny;
    assert((nx == img0->nx) && (ny == img0->ny)); 
    assert((nx == img1->nx) && (ny == img1->ny)); 
    for (int y = 0; y < ny; y++) {
      for (int x = 0; x < nx; x++) {
        int id = y * nx + x;
	double p0 = dsm_image_floatize(img0->p[id], 0.0, 1.0);
	double p1 = dsm_image_floatize(img1->p[id], 0.0, 1.0);
        diff->p[id] = dsm_image_quantize(scale*(p0 - p1), -1.0, +1.0);  
      }   
    }
  }

void dsm_image_extract_band(dsm_image_t *img, int y0, dsm_image_t *band) 
  {
    int nx = img->nx;
    assert (band->nx == nx);
    assert ((band->ny % 2) == 1);
    int yrad = band->ny / 2;
    assert ((yrad <= y0) && (y0 + band->ny - 1 - yrad < img->ny));
 
    for (int y_new = 0; y_new < band->ny; y_new++) {
      int y_old = y0 + y_new - yrad;
      for (int x_new = 0; x_new < nx; x_new++) {
	int x_old = x_new;
	band->p[y_new * nx + x_new] = img->p[y_old * nx + x_old];  
      }   
    }
  }

void dsm_image_shrink (
   dsm_image_t *img, 
   int xreduc, 
   int yreduc, 
   int xnlines, 
   int ynlines, 
   dsm_image_t *red ) 
  {
   assert ((img->nx % xreduc) == 0);
   assert ((img->ny % yreduc) == 0);
   assert (red->nx == img->nx/xreduc);
   assert (red->ny == img->ny/yreduc);

   assert((xnlines % 2) == 1);
   int rx = xnlines/2;

   assert((ynlines % 2) == 1);
   int ry = ynlines/2;

   /* !!! Should pass as parameters !!! */
   double *wx = dsm_image_compute_hahn_weights (xnlines); 
   double *wy = dsm_image_compute_hahn_weights (ynlines); 

   for (int ynew = 0; ynew < red->ny; ynew++) {
      int yold = ynew * yreduc + yreduc/2;
      assert (yold < img->ny);
      for (int xnew = 0; xnew < red->nx; xnew++) {
         int xold = xnew * xreduc + xreduc/2;
         assert (xold < img->nx);
         double sum_wp = 0.0;
         double sum_w = 0.0;
         for (int ix = -rx; ix <= +rx; ix++) {
            for (int iy = -ry; iy <= +ry; iy++) {
               int kx = xold + ix; 
               int ky = yold + iy; 
               if ((kx >= 0) && (kx < img->nx) && (ky >= 0) && (ky < img->ny)) {
                  double p = (double)img->p[ky * img->nx + kx];
                  double w = wx[ix + rx] * wy[iy + ry];
                  sum_wp += w * p;
                  sum_w += w;
               }
            }
         }
         double p_new = (sum_w == 0 ? 128 : sum_wp/sum_w);
         red->p[ynew * red->nx + xnew] = dsm_image_quantize(p_new, 0, 255);
      }
   }
   free(wx);
   free(wy);
}

void dsm_image_blur (dsm_image_t *img, int rx, int ry, dsm_image_t *blur) {

   int nkx = 2*rx + 1;
   int nky = 2*ry + 1;
  
   /* !!! Should pass as parameters !!! */
   double *wx = dsm_image_compute_hahn_weights (nkx); 
   double *wy = dsm_image_compute_hahn_weights (nky);

   for (int y = 0; y < img->ny; y++) {
      for (int x = 0; x < img->nx; x++) {
         double p = dsm_image_compute_local_avg (img, x, y, rx, wx, ry, wy);
         blur->p[y * img->nx + x] = dsm_image_quantize(p, 0.0, 255.0); 
      }
   }
   free(wx);
   free(wy);
}

void dsm_image_high_pass (dsm_image_t *img, int rx, int ry, dsm_image_t *high)
  { 
    int nx = img->nx;
    int ny = img->ny;
    assert((nx == high->nx) && (ny == high->ny)); 

    dsm_image_blur (img, rx, ry, high);

    for (int y = 0; y < ny; y++) {
      for (int x = 0; x < nx; x++) {
	double p = img->p[y * nx + x];
        double q = high->p[y * nx + x];
        double d = 127.0 + (p - q)/2;
	high->p[y * nx + x] = dsm_image_quantize(d, 0.0, 255.0); 
      }
    }
  }

void dsm_image_log_high_pass(dsm_image_t *img, int rx, int ry, dsm_image_t *high, dsm_image_t *temp)
  { 
    double logA = 0.5;
    double logB = 1.0;
    dsm_image_to_logscale(img, logA, logB, temp);
    dsm_image_high_pass(temp, rx, ry, high);
    dsm_image_to_linscale(high, logA, logB, high);
  }

void dsm_image_to_logscale(dsm_image_t *img, double A, double B, dsm_image_t *imlog) 
  {
    int nx = img->nx;
    int ny = img->ny;
    assert((nx == imlog->nx) && (ny == imlog->ny)); 
    for (int y = 0; y < ny; y++) {
      for (int x = 0; x < nx; x++) {
	double p = dsm_image_floatize(img->p[y * nx + x], 0.0, 1.0);
        double q = p == 0 ? 0 : A*log(p) + B;
	imlog->p[y * nx + x] = dsm_image_quantize(q, 0.0, 1.0); 
      }
    }
  }    

void dsm_image_to_linscale(dsm_image_t *img, double A, double B, dsm_image_t *imlin) 
  {
    int nx = img->nx;
    int ny = img->ny;
    assert((nx == imlin->nx) && (ny == imlin->ny)); 
    for (int y = 0; y < ny; y++) {
      for (int x = 0; x < nx; x++) {
	double p = dsm_image_floatize(img->p[y * nx + x], 0.0, 1.0);
        double q = exp((p - B)/A);
	imlin->p[y * nx + x] = dsm_image_quantize(q, 0.0, 1.0); 
      }
    }
  }    

uint8_t dsm_image_quantize (double p, double pmin, double pmax) {
  int pint = (int)((p - pmin)*(255.0/(pmax - pmin)) + 0.5);
  if (pint < 0) { pint = 0; }
  else if (pint > 255) { pint = 255; }
  return (uint8_t)pint;
}

double dsm_image_floatize (uint8_t pint, double pmin, double pmax) {
  double p = pmin + ((double)pint)*((pmax - pmin)/255.0);
  return p;
}

double* dsm_image_compute_hahn_weights (int n) {

   assert((n % 2) == 1);

   double *w = (double*)malloc(n*sizeof(double));
 
   int r = n/2;
   for (int i = -r; i <= +r; i++) {
      w[r+i] = (1.0 + cos(M_PI * i/(r + 0.5)))/2.0;
   }
   return w;
}

double dsm_image_compute_local_avg (
   dsm_image_t *img,
   int x, 
   int y,
   int rx,
   double wx[],
   int ry,
   double wy[])
 {
   double sum_wp = 0.0;
   double sum_w = 0.0;
   for (int ix = -rx; ix <= +rx; ix++) {
     for (int iy = -ry; iy <= +ry; iy++) {
       int kx = x + ix; 
       int ky = y + iy; 
       if ((kx >= 0) && (kx < img->nx) && (ky >= 0) && (ky < img->ny) && (kx != x) && (ky != y)) {
	 double p = (double)img->p[ky * img->nx + kx];
	 double w = wx[ix + rx] * wy[iy + ry];
	 sum_wp += w * p;
	 sum_w += w;
       }
     }
   }
   double avg_p = (sum_w == 0 ? 128 : sum_wp/sum_w);
   return avg_p; 
 }

void dsm_image_median_filter (dsm_image_t *img, dsm_image_t *res, int radius) {

   int diam = 2*radius + 1;
   int vector[diam*diam];

   assert((img->nx == res->nx) && (img->ny == res->ny));

   for (int y = 0; y < img->ny; y++) {
      for (int x = 0; x < img->nx; x++) {
         int size = 0;
         for (int i = -radius; i <= +radius; i++) {
             for (int j = -radius; j <= +radius; j++) {
                 int xi = x + i;
                 int yj = y + j;
                 if (xi >= 0 && (xi < img->nx) && (yj >= 0) && (yj < img->ny)) {
                     vector[size] = img->p[yj * img->nx + xi];
                     size++;
                 }
             }
         }
         qsort(vector, size, sizeof(int), dsm_image_compare_pixels);
         res->p[y * img->nx + x] = (uint8_t)(vector[size/2]);
      }
   }
}

int dsm_image_compare_pixels(const void *p, const void *q)
  {
    int ret;
    int x = *(const int *)p;
    int y = *(const int *)q;

    /* Avoid return x - y, which can cause undefined behaviour
       because of signed integer overflow. */
    if (x == y)
      ret = 0;
    else if (x < y)
      ret = -1;
    else
      ret = 1;

    return ret;
  }

double dsm_image_min (dsm_image_t *img) {
   double pmin = HUGE_VAL;
   for (int y = 0; y < img->ny; y++) {
      for (int x = 0; x < img->nx; x++) {
         double p = (double)img->p[y * img->nx + x];
         if (p < pmin) {
            pmin = p;
         } 
      }
   }
   return pmin;
}

double dsm_image_min_band (dsm_image_t *img, int by, int ey) 
  {
    assert((0 <= by) && (by <= ey) && (ey < img->ny)); 
    double pmin = HUGE_VAL;
    for (int y = by; y < ey; y++) {
      for (int x = 0; x < img->nx; x++) {
	double p = (double)img->p[y * img->nx + x];
	if (p < pmin) {
	  pmin = p;
	} 
      }
    }
    return pmin;
  }

void dsm_image_read_pnm_header(FILE *rd, int *magic, int *ncols, int *nrows, int *maxval)
  {
    /* Read magic number line */
    int C = fgetc(rd);
    assert(C == 'P');
    C = fgetc(rd);;
    assert ((C == '2') || (C == '6') || (C == '5') || (C == '3'));
    (*magic) = C - '0';

    /* Skip whitespace: */
    do { C = fgetc(rd); } while ((C == ' ') || (C =='\011') || (C == '\015'));
    assert(C == '\n');
 
    /* Skip possible comment line: */
    C = fgetc(rd);
    if (C == '#') {
      /* Skip comment line: */
      do { C = fgetc(rd); } while ((C != EOF) && (C != '\n'));
    } else {
      ungetc(C, rd);
    }

    /* Read image size: */
    int res = fscanf(rd, "%d %d", ncols, nrows);
    assert(res == 2);
    assert((*ncols >= 0) && (*nrows >= 0) && (*ncols <= 10000) && (*nrows <= 10000));
    do { C = fgetc(rd); } while (C == ' ');
    assert(C == '\n');
	
    /* Read maxval: */
    res = fscanf(rd, "%d", maxval);
    assert(res == 1);
    if (*maxval != 255) { fprintf(stderr, "%s: Maxval is not 255, but %d", __FUNCTION__, *maxval); }
  }

void dsm_image_read_pgm_header(FILE *rd, int *magic, int *ncols, int *nrows, int *maxval)
  {
    dsm_image_read_pnm_header(rd, magic, ncols, nrows, maxval);
    assert((*magic == 5) || (*magic == 2));
  }


void dsm_image_read_ppm_header(FILE *rd, int *magic, int *ncols, int *nrows, int *maxval)
  {
    dsm_image_read_pnm_header(rd, magic, ncols, nrows, maxval);
    assert(*magic == 6);
  }

void dsm_image_read_pgm_raw(FILE *rd, dsm_image_t *img) 
  {
    int magic, maxval;
    int nx, ny;
    dsm_image_read_pgm_header(rd, &magic, &nx, &ny, &maxval);
    assert(magic == 5);
    dsm_image_realloc(img, nx, ny, 0);

    /* Read binary image data */
    uint8_t *tmpptr = img->p;
    for (int i = 0; i < ny; i++) {
      size_t size = fread(tmpptr, nx, 1, rd);
      assert(size == 1);
      tmpptr += nx;
    }
  }

void dsm_image_read_pgm_ascii(FILE *rd, dsm_image_t *img) 
  {
    int magic, maxval;
    int nx, ny;
    dsm_image_read_pgm_header(rd, &magic, &nx, &ny, &maxval);
    assert(magic == 2);
    dsm_image_realloc(img, nx, ny, 0);

    int size = nx*ny;
    for (int i = 0; i < size; i++) {
      int pixel;
      int res = fscanf (rd, "%d", &pixel);
      assert(res == 1);
      assert((pixel >= 0) && (pixel <= maxval));
      img->p[i] = (uint8_t)pixel;
    }
  }

void dsm_image_write_pgm_raw(FILE *wr, dsm_image_t *img)
  {
    int nx = img->nx;
    int ny = img->ny;

    /* Write header */
    fprintf(wr, "P5\n");
    fprintf(wr, "%d %d\n", nx, ny);
    fprintf(wr, "255\n");
    fflush(wr);

    /* Write binary data */
    uint8_t *p = img->p;
    for (int y = 0 ; y < ny ; y++)  {
      fwrite(p, nx, 1, wr);
      p += nx;
    }
  }

void dsm_image_write_ppm_raw(FILE *wr, dsm_image_t *imgR, dsm_image_t *imgG, dsm_image_t *imgB)
  {
    int nx = imgR->nx;
    int ny = imgR->ny;
    assert((nx == imgG->nx) && (ny == imgG->ny)); 
    assert((nx == imgB->nx) && (ny == imgB->ny)); 

    /* Write header */
    fprintf(wr, "P6\n");
    fprintf(wr, "%d %d\n", nx, ny);
    fprintf(wr, "255\n");
    fflush(wr);

    /* Write binary data */
    uint8_t *pR = imgR->p;
    uint8_t *pG = imgR->p;
    uint8_t *pB = imgR->p;
    for (int y = 0 ; y < ny ; y++)  {
      for (int x = 0 ; x < nx ; x++)  {
	fwrite(pR, 1, 1, wr); 
	fwrite(pG, 1, 1, wr);
	fwrite(pB, 1, 1, wr);
	pR++;  pG++;  pB++;
      }
    }
  }

void dsm_image_debug(dsm_image_t *img, const char *prefix, int frame, int iter, const char *tag)
  { bool_t debug = TRUE;
    char *cframe = (char *)"";
    if (frame >= 0) { int res = char *cframe = jsprintf("_%06d", frame); assert(res > 0); }
    char *citer = (char *)"";
    if (iter >= 0) { int res = char *citer = jsprintf("_%04d", iter); assert(res > 0); }
    { int res = char *fname = jsprintf("%s%s%s_%s.pgm", prefix, cframe, citer, tag);
      assert(res > 0);
    }
    if (debug) { fprintf(stderr, "writing file %s ...", fname); }
    FILE *wr = fopen(fname, "w");
    assert(wr != NULL);
    dsm_image_write_pgm_raw(wr, img);
    fclose(wr);
    if (debug) { fprintf(stderr, " done\n"); }
    free(fname);
    if (frame >= 0) { free(cframe); }
    if (iter >= 0) { free(citer); }
  }

void dsm_image_erode (dsm_image_t *img, int r, dsm_image_t *temp)
  {
    int nx = img->nx;
    int ny = img->ny;
    assert((nx == temp->nx) && (ny == temp->ny));
  
    int *dk = NULL; /* Index displacements by vector in structuring element. */
    int nk; /* Index displacements are {dk[0..nk-1]}. */
    dsm_image_compute_disk_displacements(nx, ny, r, &dk, &nk);
    
    int n = nx*ny;
    for (int id = 0; id < n; id++) {
      int pmin = INT_MAX;
      for (int k = 0; k < nk; k++) {
	int idk = id + dk[k];
	if ((idk >= 0) && (idk < n)) {
	  uint8_t pk = img->p[idk];
	  if (pk < pmin) { pmin = pk; }
	}
      }
      assert((pmin >= 0) && (pmin <= 255));
      temp->p[id] = (uint8_t)pmin;
    }
    dsm_image_copy(temp, img);   
    free(dk);
  }

void dsm_image_dilate (dsm_image_t *img, int r, dsm_image_t *temp)
  {
    int nx = img->nx;
    int ny = img->ny;
    assert((nx == temp->nx) && (ny == temp->ny));
  
    int *dk = NULL; /* Index displacements by vector in structuring element. */
    int nk; /* Index displacements are {dk[0..nk-1]}. */
    dsm_image_compute_disk_displacements(nx, ny, r, &dk, &nk);
    
    int n = nx*ny;
    for (int id = 0; id < n; id++) {
      int pmax = 0;
      for (int k = 0; k < nk; k++) {
	int idk = id + dk[k];
	if ((idk >= 0) && (idk < n)) {
	  uint8_t pk = img->p[idk];
	  if (pk > pmax) { pmax = pk; }
	}
      }
      assert((pmax >= 0) && (pmax <= 255));
      temp->p[id] = (uint8_t)pmax;
    }
    dsm_image_copy(temp, img);   
    free(dk);
  }


void dsm_image_compute_disk_displacements(int nx, int ny, int r, int **dkP, int *nkP)
  { 
   int diam = 2*r + 1;
    int maxnk = diam*diam;
    int *dk;
    int nk = 0;
    if ((*dkP) != NULL) { 
      dk = (*dkP); 
    } else { 
      dk = (int *)malloc(maxnk*sizeof(int));
    }
    assert(dk != NULL);
    for (int y = -r; y <= +r; y++) {
      for (int x = -r; x <= +r; x++) {
        if (x*x + y*y <= r*r) {
	  assert(nk < maxnk);
          dk[nk] = y*nx + x;
	  nk++;
	}
      }
    }
    (*dkP) = dk;
    (*nkP) = nk;
  }