#include <math.h>

#include <stdio.h>


#include "myvec.h"

#include "imatrix.h"

#include "ETF.h"

#include "fdog.h"


#include "stackmem.h"


#ifdef MEMDEBUG

#include "mnemosyne.h"

#endif


extern int debug;

#define PI 3.141592653589793

#define ABS(x) ( ((x)>0) ? (x) : (-(x)) )

#define round(x) ((int) ((x) + 0.5))


double gauss(double x, double mean, double sigma)
{
  return ( exp( (-(x-mean)*(x-mean)) / (2*sigma*sigma) ) / sqrt(PI * 2.0 * sigma*sigma) );
}

myvec *MakeGaussianVector(double sigma)
{
  myvec *GAU;
  int i, j;
  double threshold = 0.001;

#ifdef NEVER

  for (i = 1; gauss((double)i, 0.0, sigma) > threshold; i++);
#else

  i = 0;
  while(1) {
    i++;
    if ( gauss((double)i, 0.0, sigma) < threshold )
      break;
  }
#endif


  GAU = myvec_new(i+1); 
  GAU->p[0] = gauss((double)0.0, 0.0, sigma);
  for (j = 1; j < myvec_getMax(GAU); j++) GAU->p[j] = gauss((double)j, 0.0, sigma);
  if (debug) fprintf(stderr, "MakeGaussianVector(%f)[%d]: %f\n", sigma, myvec_getMax(GAU), myvec_crc(GAU));
  return GAU;
}

void GetDirectionalDoG(imatrix *image, ETF *e, mymatrix *dog, myvec *GAU1, myvec *GAU2, double tau)
{
  /*myvec *vn = myvec_new(2);*/
  double vn[2] = {0.0, 0.0};
  double x, y, d_x, d_y;
  double weight1, weight2, w_sum1, sum1, sum2, w_sum2;
  int s;
  int x1, y1;
  int i, j;
  int dd;
  double val;
  int half_w1, half_w2;
  int image_x, image_y;
  
  half_w1 = myvec_getMax(GAU1)-1;
  half_w2 = myvec_getMax(GAU2)-1;
  
  image_x = imatrix_getRow(image);
  image_y = imatrix_getCol(image);

  if (debug) fprintf(stderr, "GetDirectionalDoG: image %d e %f dog %f\n", imatrix_crc(image), ETF_crc(e), mymatrix_crc(dog));
  
  for (i = 0; i < image_x; i++) {
    for (j = 0; j < image_y; j++) {
      sum1 = sum2 = 0.0;
      w_sum1 = w_sum2 = 0.0;
      weight1 = weight2 = 0.0;
      
      vn[0] = -e->p[i][j].ty;
      vn[1] = e->p[i][j].tx;
      
      if (vn[0] == 0.0 && vn[1] == 0.0) {
	sum1 = 255.0;
	sum2 = 255.0;
	dog->p[i][j] = sum1 - tau * sum2;
	continue;
      }
      d_x = i; d_y = j;
      /* ////////////////////////////////// */
      for (s = -half_w2; s <= half_w2; s++) { 
	/* ////////////////// */
	x = d_x + vn[0] * s;
	y = d_y + vn[1] * s;
	/* /////////////////////////////////////////////// */
	if (x > (double)image_x-1 || x < 0.0 || y > (double)image_y-1 || y < 0.0) 
	  continue;
	x1 = round(x);	if (x1 < 0) x1 = 0; if (x1 > image_x-1) x1 = image_x-1;
	y1 = round(y);	if (y1 < 0) y1 = 0; if (y1 > image_y-1) y1 = image_y-1;
	val = image->p[x1][y1];
	/* /////////////////////////////////////////////////// */
	dd = ABS(s);
	if (dd > half_w1) weight1 = 0.0;
	else weight1 = GAU1->p[dd];
	/* //////////////////////////// */
	sum1 += val * weight1;
	w_sum1 += weight1;
	/* /////////////////////////////////////////////// */
	weight2 = GAU2->p[dd];
	sum2 += val * weight2;
	w_sum2 += weight2;
      }
      /* /////////////////// */
      sum1 /= w_sum1; 
      sum2 /= w_sum2; 
      /* //////////////////////////////// */
      dog->p[i][j] = sum1 - tau * sum2;
    }
  }
  release(vn);
}

void GetFlowDoG(ETF *e, mymatrix *dog, mymatrix *tmp, myvec *GAU3)
{
  myvec *vt = myvec_new(2);
  double x, y, d_x, d_y;
  double weight1, w_sum1, sum1;
  int i_x, i_y, k;
  int x1, y1;
  double val;
  int i, j;
  int image_x = mymatrix_getRow(dog);
  int image_y = mymatrix_getCol(dog);
  int half_l;
  double step_size = 1.0; 
  /* int flow_DOG_sign = 0; */
  
  half_l = myvec_getMax(GAU3)-1;

  for (i = 0; i < image_x; i++) {
    for (j = 0; j < image_y; j++) {
      sum1 = 0.0;
      w_sum1 = 0.0;
      weight1 = 0.0;
      /* /////////////////////////// */
      val = dog->p[i][j];
      weight1 = GAU3->p[0]; 
      sum1 = val * weight1;
      w_sum1 += weight1;
      /* ////////////////////////////////////////// */
      d_x = (double)i; d_y = (double)j; 
      i_x = i; i_y = j;
      /* ////////////////////// */
      for (k = 0; k < half_l; k++) {
	vt->p[0] = e->p[i_x][i_y].tx;
	vt->p[1] = e->p[i_x][i_y].ty;
	if (vt->p[0] == 0.0 && vt->p[1] == 0.0) break;
	x = d_x;
	y = d_y;
	/* /////////////////////////////////////////////// */
	if (x > (double)image_x-1 || x < 0.0 || y > (double)image_y-1 || y < 0.0) break;
	x1 = round(x);	if (x1 < 0) x1 = 0; if (x1 > image_x-1) x1 = image_x-1;
	y1 = round(y);	if (y1 < 0) y1 = 0; if (y1 > image_y-1) y1 = image_y-1;
	val = dog->p[x1][y1];
	/* //////////////////////// */
	weight1 = GAU3->p[k]; 
	/* ////////////// */
	sum1 += val * weight1;
	w_sum1 += weight1;
	/* /////////////////////////////////// */
	d_x += vt->p[0] * step_size; 
	d_y += vt->p[1] * step_size; 
	/* /////////////////////////////////// */
	i_x = round(d_x); 
	i_y = round(d_y); 
	if (d_x < 0 || d_x > image_x-1 || d_y < 0 || d_y > image_y-1) break;
	/* /////////////////// */
      }
      /* ////////////////////////////////////////// */
      d_x = (double)i; d_y = (double)j; 
      i_x = i; i_y = j;
      for (k = 0; k < half_l; k++) {
	vt->p[0] = -e->p[i_x][i_y].tx;
	vt->p[1] = -e->p[i_x][i_y].ty;
	if (vt->p[0] == 0.0 && vt->p[1] == 0.0) break;
	x = d_x;
	y = d_y;
	/* /////////////////////////////////////////////// */
	if (x > (double)image_x-1 || x < 0.0 || y > (double)image_y-1 || y < 0.0) break;
	x1 = round(x);	if (x1 < 0) x1 = 0; if (x1 > image_x-1) x1 = image_x-1;
	y1 = round(y);	if (y1 < 0) y1 = 0; if (y1 > image_y-1) y1 = image_y-1;
	val = dog->p[x1][y1];
	/* //////////////////////// */
	weight1 = GAU3->p[k]; 
	/* ////////////// */
	sum1 += val * weight1;
	w_sum1 += weight1;
	/* /////////////////////////////////// */
	d_x += vt->p[0] * step_size; 
	d_y += vt->p[1] * step_size; 
	/* /////////////////////////////////// */
	i_x = round(d_x); 
	i_y = round(d_y); 
	if (d_x < 0 || d_x > image_x-1 || d_y < 0 || d_y > image_y-1) break;
	/* /////////////////// */
      }
      /* ////////////////////////////////// */
      sum1 /= w_sum1; 
      /* //////////////////////////////// */
      if (sum1 > 0) tmp->p[i][j] = 1.0; 
      else tmp->p[i][j] = 1.0 + tanh(sum1);
    }
  }
  release(vt);
}

void GetFDoG(imatrix *image, ETF *e, double sigma, double sigma3, double tau) 
{
  int	i, j;
  int image_x = imatrix_getRow(image);
  int image_y = imatrix_getCol(image);
  myvec *GAU1, *GAU2, *GAU3;
  int half_w1, half_w2, half_l;
  mymatrix *tmp;
  mymatrix *dog;

  if (debug) fprintf(stderr, "GetFDoG entry e %f\n", ETF_crc(e));
  
  GAU1 = MakeGaussianVector(sigma); 
  GAU2 = MakeGaussianVector(sigma*1.6); 

  if (debug) fprintf(stderr, "Gaussian1 %f\n", myvec_crc(GAU1));
  if (debug) fprintf(stderr, "Gaussian2 %f\n", myvec_crc(GAU2));
  
  half_w1 = myvec_getMax(GAU1)-1;
  half_w2 = myvec_getMax(GAU2)-1;
  
  GAU3 = MakeGaussianVector(sigma3); 
  if (debug) fprintf(stderr, "Gaussian3 %f\n", myvec_crc(GAU3));
  half_l = myvec_getMax(GAU3)-1;
  
  tmp = mymatrix_new(image_x, image_y);
  dog = mymatrix_new(image_x, image_y);
  
  GetDirectionalDoG(image, e, dog, GAU1, GAU2, tau);
  if (debug) fprintf(stderr, "GetDirectionalDoG image %d\n", imatrix_crc(image));
  if (debug) fprintf(stderr, "GetDirectionalDoG e %f\n", ETF_crc(e));
  if (debug) fprintf(stderr, "GetDirectionalDoG dog %f\n", mymatrix_crc(dog));
  GetFlowDoG(e, dog, tmp, GAU3);
  if (debug) fprintf(stderr, "GetFlowDoG e %f\n", ETF_crc(e));
  if (debug) fprintf(stderr, "GetFlowDoG dog %f\n", mymatrix_crc(dog));
  if (debug) fprintf(stderr, "GetFlowDoG tmp %f\n", mymatrix_crc(tmp));
  
  for (i = 0; i < image_x; i++) {
    for (j = 0; j < image_y; j++) image->p[i][j] = round(tmp->p[i][j] * 255.);
  }

  release(GAU1);
}

void GaussSmoothSep(imatrix *image, double sigma) /* NOT USED ??? */
{
  int	i, j;
  double g, max_g, min_g;
  int s, t;
  int x, y;
  double weight, w_sum;
  int image_x = imatrix_getRow(image);
  int image_y = imatrix_getCol(image);
  myvec *GAU1;
  int half;
  mymatrix *tmp;
  /* int MAX_GRADIENT = -1; */
  
  GAU1 = MakeGaussianVector(sigma); 
  half = myvec_getMax(GAU1)-1;
  tmp = mymatrix_new(image_x, image_y);
  max_g = -1; min_g = 10000000;
  for (j = 0; j < image_y; j++) {
    for (i = 0; i < image_x; i++) {
      g = 0.0; weight = w_sum = 0.0;
      for (s = -half; s <= half; s++) {
	x = i+s; y = j;
	if (x > image_x-1) x = image_x-1; else if (x < 0) x = 0;
	if (y > image_y-1) y = image_y-1; else if (y < 0) y = 0;
	weight = GAU1->p[ABS(s)];
	g += weight * image->p[x][y];
	w_sum += weight;
      }
      g /= w_sum;
      if (g > max_g) max_g = g;
      if (g < min_g) min_g = g;
      tmp->p[i][j] = g;
    }
  }
  for (j = 0; j < image_y; j++) {
    for (i = 0; i < image_x; i++) {
      g = 0.0;
      weight = w_sum = 0.0;
      for (t = -half; t <= half; t++) {
	x = i; y = j+t;
	if (x > image_x-1) x = image_x-1; else if (x < 0) x = 0;
	if (y > image_y-1) y = image_y-1; else if (y < 0) y = 0;
	weight = GAU1->p[ABS(t)];
	g += weight * tmp->p[x][y];
	w_sum += weight;
      }
      g /= w_sum;
      if (g > max_g) max_g = g;
      if (g < min_g) min_g = g;
      image->p[i][j] = round(g);
    }
  }
  release(GAU1);  
  /*	TRACE("max_g = %f\n", max_g); */
  /*	TRACE("min_g = %f\n", min_g); */
}

#ifdef NEVER

void ConstructMergedImage(imatrix *image, imatrix *gray, imatrix *merged) 
{
  int x, y;
  int image_x = imatrix_getRow(image), image_y = imatrix_getCol(image);
  
  for (y = 0; y < image_y; y++) {
    for (x = 0; x < image_x; x++) merged->p[x][y] = (gray->p[x][y] == 0 ? 0 : image->p[x][y]);
  }
}

void ConstructMergedImageMult(imatrix *image, imatrix *gray, imatrix *merged) /* using multiplication */
{
  int x, y;
  double gray_val, line_darkness;
  int image_x = imatrix_getRow(image), image_y = imatrix_getCol(image);

  for (y = 0; y < image_y; y++) {
    for (x = 0; x < image_x; x++) {
      gray_val = image->p[x][y] / 255.0; 
      line_darkness = gray->p[x][y] / 255.0; 
      gray_val *= line_darkness; 
      merged->p[x][y] = round(gray_val * 255.0);
    }
  }
}

void Binarize(imatrix *image, double thres) 
{
  int	i, j;
  double val;
  int image_x = imatrix_getRow(image), image_y = imatrix_getCol(image);
  
  for (i = 0; i < image_x; i++) { 
    for (j = 0; j < image_y; j++) {
      val = image->p[i][j] / 255.0; 
      image->p[i][j] = (val < thres ? 0 : 255); 
    }
  }
}
#endif


void GrayThresholding(imatrix *image, double thres) 
{
  int	i, j;
  double val;
  
  int image_x = imatrix_getRow(image);
  int image_y = imatrix_getCol(image);
  
  for (i = 0; i < image_x; i++) { 
    for (j = 0; j < image_y; j++) {
      val = image->p[i][j] / 255.0; 
      image->p[i][j] = (val < thres ? round(val * 255.0) : 255); 
    }
  }
}