// Copyright (C) 1999-2015
// Smithsonian Astrophysical Observatory, Cambridge, MA, USA
// For conditions of distribution and use, see copyright notice in "copyright"

#include "context.h"
#include "base.h"
#include "fitsimage.h"
#include "fvcontour.h"

#include "sigbus.h"
#define INTERP (((Base*)parent)->interp)

// It is a modified version of contour code found in Fv 2.4
// Fv may be obtained from the HEASARC (High Energy Astrophysics Science
// Archive Research Center) FTOOLS Web site at:
// http://heasarc.gsfc.nasa.gov/ftools/fv.html

// The original author is unknown.

FVContour::FVContour(Base* pp, FitsImage* fits, const char* cc, int ww,
		     int dd, Method mm, int nn, int rr, const char* ll,
		     FrScale::ColorScaleType sc, float exp, 
		     float cm, Vector lim, InverseScale* ss)
  : Contour(pp, cc, ww, dd)
{
  method_ = mm;
  level_ = dupstr(ll);
  numLevel_ = nn;
  smooth_ = rr;
  colorScaleType_ = sc;
  expo_ = exp;
  clipMode_ = cm;
  limits_ = lim;

  scale = ss;

  append(fits);
}

FVContour::~FVContour()
{
  if (level_)
    delete [] level_;

  if (scale)
    delete scale;
}

void FVContour::update(FitsImage* fits)
{
  contours_.deleteAll();
  append(fits);
}

void FVContour::append(FitsImage* fits)
{
  if (smooth_ == 1)
    unity(fits);
  else
    switch (method_) {
    case SMOOTH:
      nobin(fits);
      break;
    case BLOCK:
      bin(fits);
      break;
    }
}

void FVContour::unity(FitsImage* fits)
{
  FitsBound* params = 
    fits->getDataParams(((Base*)parent)->currentContext->frScale.scanMode());
  long width = fits->width();
  long height = fits->height();

  // blank img
  long size = width*height;
  double* img = new double[size];
  if (!img) {
    internalError("FVContour could not allocate enough memory");
    return;
  }
  for (long ii=0; ii<size; ii++)
    img[ii] = FLT_MIN;

  // fill img
  SETSIGBUS
  for(long jj=params->ymin; jj<params->ymax; jj++) {
    for(long ii=params->xmin; ii<params->xmax; ii++) {
      long kk = jj*width + ii;

      double vv = fits->getValueDouble(kk);
      if (isfinite(vv))
	img[kk] = vv;
    }
  }
  CLEARSIGBUS

  // do contours
  int status = build(width, height, img, fits->dataToRef);

  // clean up
  delete [] img;

  if (status)
    internalError("Unknown FVContour error");
}

void FVContour::nobin(FitsImage* fits)
{
  long width = fits->width();
  long height = fits->height();

  // blank img
  long size = width*height;
  double* img = new double[size];
  if (!img) {
    internalError("FVContour could not allocate enough memory");
    return;
  }
  for (long ii=0; ii<size; ii++)
    img[ii] = FLT_MIN;

  // generate kernal
  int r = smooth_-1;
  double* kernal = gaussian(r);

  // convolve
  convolve(fits,kernal,img,r);
  
  // now, do contours
  int status = build(width, height, img, fits->dataToRef);

  // cleanup
  delete kernal;
  delete img;

  if (status)
    internalError("Unknown FVContour error");
}

void FVContour::convolve(FitsImage* fits, double* kernal, double* dest, int r)
{
  FitsBound* params = 
    fits->getDataParams(((Base*)parent)->currentContext->frScale.scanMode());
  long width = fits->width();
  int rr = 2*r+1;

  SETSIGBUS
  for (long jj=params->ymin; jj<params->ymax; jj++) {
    for (long ii=params->xmin; ii<params->xmax; ii++) {
      long ir  = ii-r;
      long irr = ii+r;
      long jr = jj-r;
      long jrr = jj+r;

      for (long n=jr, nn=0; n<=jrr; n++, nn++) {
	if (n>=params->ymin && n<params->ymax) {
	  for (long m=ir, mm=0; m<=irr; m++, mm++) {
	    if (m>=params->xmin && m<params->xmax) {
	      double vv = fits->getValueDouble(n*width+m);
	      if (isfinite(vv)) {
		double kk = kernal[nn*rr+mm];
		double* ptr = dest+(jj*width+ii);
		if (*ptr == FLT_MIN)
		  *ptr  = vv*kk;
		else
		  *ptr += vv*kk;
	      }
	    }
	  }
	}
      }
    }
  }
  CLEARSIGBUS
}

double* FVContour::tophat(int r)
{
  int rr = 2*r+1;
  int ksz = rr*rr;
  double* kernal = new double[ksz];
  memset(kernal, 0, ksz*sizeof(double));
  
  double kt = 0;
  for (int yy=-r; yy<=r; yy++) {
    for (int xx=-r; xx<=r; xx++) { 
      if ((xx*xx + yy*yy) <= r*r) {
	kernal[(yy+r)*rr+(xx+r)] = 1;
	kt++;
      }
    }
  }

  // normalize kernal
  for (int aa=0; aa<ksz; aa++)
    kernal[aa] /= kt;

  return kernal;
}

double* FVContour::gaussian(int r)
{
  int rr = 2*r+1;
  int ksz = rr*rr;
  double sigma = r/2.;
  double* kernal = new double[ksz];
  memset(kernal, 0, ksz*sizeof(double));
  
  double kt = 0;
  double aa = 1./(sigma*sigma);
  double cc = 1./(sigma*sigma);
  for (int yy=-r; yy<=r; yy++) {
    for (int xx=-r; xx<=r; xx++) { 
      if ((xx*xx + yy*yy) <= r*r) {
	double vv = exp(-.5*(aa*xx*xx + cc*yy*yy));
	kernal[(yy+r)*rr+(xx+r)] = vv;
	kt += vv;
      }
    }
  }

  // normalize kernal
  for (int aa=0; aa<ksz; aa++)
    kernal[aa] /= kt;

  return kernal;
}

void FVContour::bin(FitsImage* fits)
{
  FitsBound* params = 
    fits->getDataParams(((Base*)parent)->currentContext->frScale.scanMode());
  long width = fits->width();
  long height = fits->height();

  int rr = smooth_;

  long w2 = (long)(width/rr);
  long h2 = (long)(height/rr);

  Matrix m = 
    Translate((Vector(-width,-height)/2).floor()) * 
    Scale(1./rr) * 
    Translate((Vector(w2,h2)/2).floor());
  Matrix n = m.invert();
  double* mm = m.mm();

  double* img = new double[w2 * h2];
  {
    for (long jj=0; jj<h2; jj++)
      for (long ii=0; ii<w2; ii++)
	img[jj*w2 + ii] = FLT_MIN;
  }

  short* count = new short[w2 * h2];
  memset(count, 0, w2*h2*sizeof(short));

  SETSIGBUS
  for (long jj=params->ymin; jj<params->ymax; jj++) {
    for (long ii=params->xmin; ii<params->xmax; ii++) {
      double xx = ii*mm[0] + jj*mm[3] + mm[6];
      double yy = ii*mm[1] + jj*mm[4] + mm[7];

      if (xx >= 0 && xx < w2 && yy >= 0 && yy < h2) {
	long kk = (long(yy)*w2 + long(xx));
	double v = fits->getValueDouble(jj*width + ii);
	if (isfinite(v)) {
	  if (count[kk])
	    img[kk] += v;
	  else
	    img[kk] = v;

	  count[kk]++;
	}
      }
    }
  }

  for (long kk=0; kk<w2*h2; kk++)
    if (count[kk])
      img[kk] /= count[kk];

  CLEARSIGBUS

  delete [] count;

  Matrix w = n * fits->dataToRef;
  int status = build(w2, h2, img, w);

  delete [] img;

  if (status)
    internalError("Unknown FVContour error");
}

int FVContour::build(long xdim, long ydim, double *image, Matrix& mx)
{
  int status = 0;

  long nelem = xdim*ydim;
  char* usedGrid = new char[nelem];
  if (!usedGrid)
    return 1;

  double** rows = new double*[ydim];
  if (!rows)
    return 1;

  for (long jj=0; jj<ydim; jj++)
    rows[jj] = image + jj*xdim;

  for (long c=0; c<scale->size() && !status; c++) {
    double cntour = scale->level(c);
    for (long elem=0; elem<nelem; elem++)
      usedGrid[elem] = 0;

    //  Search outer edge
    long ii,jj;

    //  Search top
    for (jj=0, ii=0; ii<xdim-1 && !status; ii++)
      if (rows[jj][ii]<cntour && cntour<=rows[jj][ii+1])
	status = trace(xdim, ydim, cntour, ii, jj, top, rows, usedGrid, mx);

    //  Search right
    for (jj=0; jj<ydim-1 && !status; jj++)
      if (rows[jj][ii]<cntour && cntour<=rows[jj+1][ii])
	status = trace(xdim, ydim, cntour, ii-1, jj, right, rows, usedGrid, 
		       mx);

    //  Search Bottom
    for (ii--; ii>=0 && !status; ii--)
      if (rows[jj][ii+1]<cntour && cntour<=rows[jj][ii])
	status = trace(xdim, ydim, cntour, ii, jj-1, bottom, rows, usedGrid, 
		       mx);

    //  Search Left
    for (ii=0, jj--; jj>=0 && !status; jj--)
      if (rows[jj+1][ii]<cntour && cntour<=rows[jj][ii])
	status = trace(xdim, ydim, cntour, ii, jj, left, rows, usedGrid, mx);

    //  Search each row of the image
    for (jj=1; jj<ydim-1 && !status; jj++)
      for (ii=0; ii<xdim-1 && !status; ii++)
	if (!usedGrid[jj*xdim + ii] && rows[jj][ii]<cntour && cntour<=rows[jj][ii+1])
	  status = trace(xdim, ydim, cntour, ii, jj, top, rows, usedGrid, 
			 mx);
  }

  delete [] usedGrid;
  delete [] rows;

  return status;
}

int FVContour::trace(long xdim, long ydim, double cntr,
		     long xCell, long yCell, int side, 
		     double** rows, char* usedGrid, Matrix& mx)
{
  long ii = xCell;
  long jj = yCell;
  int origSide = side;

  int init = 1;
  int done = (ii<0 || ii>=xdim-1 || jj<0 && jj>=ydim-1);

  while (!done) {
    int flag = 0;
    double a = rows[jj][ii];
    double b = rows[jj][ii+1];
    double c = rows[jj+1][ii+1];
    double d = rows[jj+1][ii];

    double X, Y;
    if (init) {
      init = 0;
      switch (side) {
      case top:
	X = (cntr-a) / (b-a) + ii;
	Y = jj;
	break;
      case right:
	X = ii+1;
	Y = (cntr-b) / (c-b) + jj;
	break;
      case bottom:
	X = (cntr-c) / (d-c) + ii;
	Y = jj+1;
	break;
      case left:
	X = ii;
	Y = (cntr-a) / (d-a) + jj;
	break;
      }

    }
    else {
      if (side==top)
	usedGrid[jj*xdim + ii] = 1;

      do {
	if (++side == none)
	  side = top;

	switch (side) {
	case top:
	  if (a>=cntr && cntr>b) {
	    flag = 1;
	    X = (cntr-a) / (b-a) + ii;
	    Y = jj;
	    jj--;
	  }
	  break;
	case right:
	  if( b>=cntr && cntr>c ) {
	    flag = 1;
	    X = ii+1;
	    Y = (cntr-b) / (c-b) + jj;
	    ii++;
	  }
	  break;
	case bottom:
	  if( c>=cntr && cntr>d ) {
	    flag = 1;
	    X = (cntr-d) / (c-d) + ii;
	    Y = jj+1;
	    jj++;
	  }
	  break;
	case left:
	  if( d>=cntr && cntr>a ) {
	    flag = 1;
	    X = ii;
	    Y = (cntr-a) / (d-a) + jj;
	    ii--;
	  }
	  break;
	}
      } while (!flag);

      if (++side == none)
	side = top;
      if (++side == none)
	side = top;
      if (ii==xCell && jj==yCell && side==origSide)
	done = 1;
      if (ii<0 || ii>=xdim-1 || jj<0 || jj>=ydim-1)
	done = 1;
    }

    contours_.append(new Vertex(Vector(X+.5,Y+.5)*mx));

    if (done)
      contours_.append(new Vertex(DBL_MAX, DBL_MAX));
  }

  return 0;
}
