#include <numeric>
#include <stdexcept>
#include "ImageProcessor.h"
#include "Log.h"

ImageProcessor::ImageProcessor() :
  _roiW(0),
  _roiH(0),
  _backgroundAverageIntensity(0),
  _cropThreshold(0),
  _useCropThreshold(true),
  _expectedROIWidth(80),
  _expectedROIHeight(100),
  _saveBackground(false),
  _charge(1) {
  _refArray.ndims = 0; /** no reference image has been loaded */
  _backgroundArray.ndims = 0; /** no background image has been loaded */
  allocROIArray();
  setROISlices(20);
}

bool ImageProcessor::process(NDArray *pArray, bool saveAsBackground) {
  if (saveAsBackground) {
    saveBackground(pArray);
  }

  unsigned long threshold = 2700;
  if (_useCropThreshold) {
    threshold = _cropThreshold;
  }
  else {
    threshold = _backgroundAverageIntensity;
  }

  unsigned int expectedROIX = _expectedROIWidth;
  unsigned int expectedROIY = _expectedROIHeight;

  _roiErrorStatus = ROI_OK;
  bool roi = findROI(pArray, threshold, expectedROIX, expectedROIY);
  if (roi) {
    saveROI(pArray);
    roi = subtractBackground();
    if (roi) {
      sliceROI();
      calculateEnergy();
    }
  }
  return roi;
}

void ImageProcessor::setExpectedROISize(unsigned int width, unsigned int height) {
  _expectedROIHeight = height;
  _expectedROIWidth = width;
}

void ImageProcessor::setCropThreshold(unsigned long cropThreshold) {
  if (_cropThreshold != cropThreshold) {
    Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpInfo
		       << "INFO: Changing crop threshold from "
		       << _cropThreshold << " to "
		       << cropThreshold << Log::dp;
  }


  _cropThreshold = cropThreshold;
}

ROIErrorStatus ImageProcessor::getROIStatus() {
  return _roiErrorStatus;
}

/**
 * Load a background image from a file. The image must have the same
 * dimensions of the image read by the camera.
 * 
 * @param fileName file name (absolute or relative path) of the 
 * background TIFF file
 *
 * @return true if background image was loaded successfuly, false
 * if file is not available or image dimensions are not correct
 */
bool ImageProcessor::loadBackground(std::string fileName) {
  bool val = loadImageFromFile(fileName, &_backgroundArray);
  if (val != true) {
    return false;
  }

  if (saveBackgroundThreshold() == false) {
    return false;
  }

  return true;
}

/**
 * Save the current background image to the specified file name.
 *
 * @param filename name of the file that will contain the
 * background image
 *
 * @return true if image was saved, false if it is not possible
 * to write the file
 */
bool ImageProcessor::saveBackground(std::string fileName) {
  TIFF *tiff = TIFFOpen(fileName.c_str(), "w");
  if (tiff == NULL) {
    return false;
  }

  TIFFSetField(tiff, TIFFTAG_IMAGEWIDTH, _backgroundArray.dims[0].size);
  TIFFSetField(tiff, TIFFTAG_IMAGELENGTH, _backgroundArray.dims[1].size);
  TIFFSetField(tiff, TIFFTAG_BITSPERSAMPLE, 16);
  TIFFSetField(tiff, TIFFTAG_SAMPLESPERPIXEL, 1);
  TIFFSetField(tiff, TIFFTAG_PHOTOMETRIC, PHOTOMETRIC_MINISBLACK);

  tsize_t scanline = TIFFScanlineSize(tiff);
  
  tdata_t buf;
  buf = _TIFFmalloc(scanline);
  uint16 *buf16;
  uint16 *pData16 = (uint16 *) _backgroundArray.pData;

  for (uint32 row = 0; row < _backgroundArray.dims[1].size; ++row) {
    buf16 = (uint16 *) buf;
    for (unsigned int i = 0; i < _backgroundArray.dims[0].size; ++i) {
      buf16[i] = pData16[i];
    } 

    if (TIFFWriteScanline(tiff, buf, row) <= 0) {
      Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpError
			 << "ERROR: Failed to write background image at row " << (int) row
			 << Log::dp;
      return false;
    }

    pData16 += _backgroundArray.dims[0].size;
  }

  TIFFClose(tiff);

  Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpInfo
		     << "INFO: Saved background image file: " << fileName.c_str()
		     << Log::dp;

  return true;
}

/**
 * Calculate a threshold used to find the ROI from samples within the
 * background image.
 */
bool ImageProcessor::saveBackgroundThreshold() {
  if (_backgroundArray.ndims != 2) {
    Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpError
		       << "ERROR: Failed on saveBackgroundThreshold()"
		       << " - no background loaded." << Log::dp;
    return false;
  }

  unsigned int size = 25;

  // Upper left square
  unsigned int leftX = (_backgroundArray.dims[0].size / 4) - (size / 2);
  unsigned int topY = (_backgroundArray.dims[1].size / 4) - (size / 2);
  unsigned long squareAverage = getSquareAverage(leftX, topY, size);
//   if (squareAverage == 0) {
//     return false;
//   }
  unsigned long sum = squareAverage;
  
  // Upper right square
  leftX += (_backgroundArray.dims[0].size / 2);
  squareAverage = getSquareAverage(leftX, topY, size);
//   if (squareAverage == 0) {
//     return false;
//   }
  sum += squareAverage;

  // Bottom right square
  topY += (_backgroundArray.dims[1].size / 2);
  squareAverage = getSquareAverage(leftX, topY, size);
//   if (squareAverage == 0) {
//     return false;
//   }
  sum += squareAverage;

  // Bottom left square
  leftX -= (_backgroundArray.dims[0].size / 2);
  squareAverage = getSquareAverage(leftX, topY, size);
//   if (squareAverage == 0) {
//     return false;
//   }
  sum += squareAverage;

  _backgroundAverageIntensity = sum / 4;

  Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpInfo
		     << "INFO: average background intensity is "
		     << _backgroundAverageIntensity << Log::dp;

  return true;
}

unsigned long ImageProcessor::getSquareAverage(unsigned int leftX, unsigned int topY,
					       unsigned int size) {
  if (_backgroundArray.ndims != 2) {
    Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpError
		       << "ERROR: Failed on getSquereAverage() - no background loaded." << Log::dp;
    return 0;
  }

  if (leftX + size > _backgroundArray.dims[0].size) {
    Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpError
		       << "ERROR: Failed on getSquereAverage(). "
		       << "Check 'leftX + size > width' failed ("
		       << leftX << " + " << size << " > "
		       << _backgroundArray.dims[0].size << ")" << Log::dp;
    return 0;
  }

  if (topY + size > _backgroundArray.dims[1].size) {
    Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpError
		       << "ERROR: Failed on getSquereAverage(). "
		       << "Check 'topY + size > height' failed ("
		       << topY << " + " << size << " > "
		       << _backgroundArray.dims[1].size << ")" << Log::dp;
    return 0;
  }

  unsigned long sum = 0;

  epicsUInt32 w = size; // X dimension
  epicsUInt32 h = size; // Y dimension

  // Point to the top/left corner
  epicsUInt16 *rowPtr = (epicsUInt16 *) _backgroundArray.pData;
  rowPtr += leftX;
  rowPtr += (topY * _backgroundArray.dims[0].size);
 
  for (unsigned int i = 0; i < h; ++i) {
    _imageRow.assign(rowPtr, rowPtr + w);
    sum += std::accumulate(_imageRow.begin(), _imageRow.end(), 0);

    rowPtr += _backgroundArray.dims[0].size;
  }

  sum /= (size * size);

  Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpInfo
		     << "INFO: average square intensity is "
		     << sum << Log::dp;
  
  return sum;
}


/**
 * Read a TIFF reference image file and saves in the _refArray attribute
 *
 * @param fileName file name (absolute or relative path) of the reference
 * image TIFF file.
 *
 * @return true if file is loaded, false if file is not found or it is not
 * in TIFF format.
 */
bool ImageProcessor::loadReference(std::string fileName) {
  return loadImageFromFile(fileName, &_refArray);
}

bool ImageProcessor::loadImageFromFile(std::string fileName, NDArray *pArray) {
  TIFF *tiff = TIFFOpen(fileName.c_str(), "r");

  if (tiff == NULL) {
    Log::getInstance() << Log::showtime << Log::flagReference << Log::dpError
		       << "ERROR: Failed to open reference image file "
		       << "\"" << fileName.c_str() << "\"" << Log::dp;
    return false;
  }

  Log::getInstance() << Log::showtime << Log::flagReference << Log::dpError
		     << "INFO: Loading reference image from file "
		     << "\"" << fileName.c_str() << "\"" << Log::dp;

  if (!loadTiffImage(tiff, pArray)) {
    return false;
  }

  TIFFClose(tiff);

  return true;
}

void ImageProcessor::allocROIArray() {
  _roiArray.ndims = 2;

  _roiArray.dims[0].size = MAX_ROI_W;
  _roiArray.dims[0].offset = 0;
  _roiArray.dims[0].binning = 0;
  _roiArray.dims[0].reverse = 0;

  _roiArray.dims[1].size = MAX_ROI_H;
  _roiArray.dims[1].offset = 0;
  _roiArray.dims[1].binning = 0;
  _roiArray.dims[1].reverse = 0;

  _roiArray.dataType = NDUInt16;
  _roiArray.dataSize = MAX_ROI_W * MAX_ROI_H * 4;
  _roiArray.pData = realloc(_roiArray.pData, _roiArray.dataSize);
}

/**
 * Load a TIFF reference image to a NDArray struct (_refArray).
 */
bool ImageProcessor::loadTiffImage(TIFF *tiff, NDArray *pArray) {
  unsigned int w;
  unsigned int h;

  TIFFGetField(tiff, TIFFTAG_IMAGEWIDTH, &w);
  TIFFGetField(tiff, TIFFTAG_IMAGELENGTH, &h);

  Log::getInstance() << Log::showtime << Log::flagGlobal << Log::dpInfo
		     << "INFO: Reference image size width=" << w << " height=" << h << Log::dp;

  pArray->ndims = 2;

  pArray->dims[0].size = w;
  pArray->dims[0].offset = 0;
  pArray->dims[0].binning = 0;
  pArray->dims[0].reverse = 0;

  pArray->dims[1].size = h;
  pArray->dims[1].offset = 0;
  pArray->dims[1].binning = 0;
  pArray->dims[1].reverse = 0;

  pArray->dataType = NDUInt16;
  pArray->dataSize = w * h * 4;
  pArray->pData = realloc(pArray->pData, pArray->dataSize);

  tsize_t scanline = TIFFScanlineSize(tiff);
  
  tdata_t buf;
  buf = _TIFFmalloc(scanline);
  uint16 *buf16;
  uint16 *pData16 = (uint16 *) pArray->pData;

  for (uint32 row = 0; row < h; ++row) {
    if (TIFFReadScanline(tiff, buf, row) <= 0) {
      Log::getInstance() << Log::showtime << Log::flagReference << Log::dpError
			 << "ERROR: Failed to read reference image at row " << (int) row
			 << Log::dp;
      return false;
    }
    buf16 = (uint16 *) buf;
    for (unsigned int i = 0; i < w; ++i) {
      pData16[i] = buf16[i];
    } 

    if (row < 3 || row > h - 4 ||
	(h / 2 -1 <= row && row <= h / 2 + 1)) {
      Log::getInstance() << Log::showtime << Log::flagReference << Log::dpDebug
			 << "[row " << (int) row << "]: "
			 << (int) pData16[0] << " "
			 << (int) pData16[1] << " "
			 << (int) pData16[2] << " ... "
			 << (int) pData16[w/2-1] << " "
			 << (int) pData16[w/2] << " "
			 << (int) pData16[w/2+1] << " ... "	
			 << (int) pData16[w-3] << " "
			 << (int) pData16[w-2] << " "
			 << (int) pData16[w-1] << " "
			 << Log::dp;
    }
    pData16 += w;
  }

  _TIFFfree(buf);

  return true;
}

/**
 * Calculate the average background pixel intensity of the reference
 * image by calculating the sums (in X/Y), sorting them and picking
 * the median sum. The average is considered the sum in X (cols) of
 * the median column divided by the image height.
 */
unsigned long ImageProcessor::findAverageIntensity() {
  if (_refArray.ndims != 2) {
    return 0;
  }

  if (!calculateSums(&_refArray)) {
    return 0;
  }

  std::sort(_sumX.begin(), _sumX.end());

  unsigned long sum = _sumX[_sumX.size()/2];
  unsigned long average = sum / _refArray.dims[1].size;

  Log::getInstance() << Log::showtime << Log::flagReference << Log::dpError
		     << "INFO: Background average intensity is "
		     << (unsigned int) average << " (sum = "
		     << (unsigned int) sum << ")" << Log::dp;
  
  return average;
}

/**
 * Update the _referenceSum vectors with information from the reference
 * image loaded into the _refArray attribute
 */
bool ImageProcessor::processReference() {
  if (_refArray.ndims == 2) {
    return processReference(&_refArray);
  }
  else {
    Log::getInstance() << Log::showtime << Log::flagReference << Log::dpError
		       << "ERROR: Cannot process reference image - no image loaded from file."
		       << Log::dp;
    return false;
  }
}

/**
 * Update the _referenceSum vectors.
 */
bool ImageProcessor::processReference(NDArray *pArray) {
  return calculateSums(pArray, _referenceSum0, _referenceSum1, _referenceSum2);
}

struct SumRow : public std::binary_function<double,double,double> {
  int row;
  double operator() (double sum, double pixel) {
    return sum + pixel * row;
  }
};

struct SumRowSquared : public std::binary_function<double,double,double> {
  int row;
  double operator() (double sum, double pixel) {
    return sum + (pixel*pixel * row*row);
  }
};

bool ImageProcessor::calculateSums(NDArray *pArray, std::vector<double> &sum0,
				   std::vector<double> &sum1, std::vector<double> &sum2) {
  epicsUInt32 w = pArray->dims[0].size;
  epicsUInt16 *rowPtr = (epicsUInt16 *) pArray->pData;
  _imageRow.assign(rowPtr, rowPtr + w);
 
  Log::getInstance() << Log::showtime << Log::flagReference << Log::dpDebug
		     << (int) _imageRow[0] << " "
		     << (int) _imageRow[1] << " "
		     << (int) _imageRow[2] << " ... "
		     << (int) _imageRow[w/2-1] << " "
		     << (int) _imageRow[w/2] << " "
		     << (int) _imageRow[w/2+1] << " ... "	
		     << (int) _imageRow[w-3] << " "
		     << (int) _imageRow[w-2] << " "
		     << (int) _imageRow[w-1] << " "
		     << Log::dp;

  sum0.resize(w);
  sum1.resize(w);
  sum2.resize(w);

  std::fill(sum0.begin(), sum0.end(), 0);
  std::fill(sum1.begin(), sum1.end(), 0);
  std::fill(sum2.begin(), sum2.end(), 0);

  SumRow sumRow;
  sumRow.row = 1;
  SumRowSquared sumRowSquared;
  sumRowSquared.row = 1;

  for (unsigned int i = 0; i < pArray->dims[1].size; ++i) {
    _imageRow.assign(rowPtr, rowPtr + w);
    std::transform(sum0.begin(), sum0.end(),
		   _imageRow.begin(), sum0.begin(), std::plus<double>());

    std::transform(sum1.begin(), sum1.end(),
		   _imageRow.begin(), sum1.begin(), sumRow);
    sumRow.row++;

    std::transform(sum2.begin(), sum2.end(),
		   _imageRow.begin(), sum2.begin(), sumRowSquared);
    sumRowSquared.row++;

    rowPtr += w;
  }

  Log::getInstance() << Log::showtime << Log::flagReference << Log::dpInfo
		     << (int) sum0[0] << " "
		     << (int) sum0[1] << " "
		     << (int) sum0[2] << " ... "
		     << (int) sum0[w/2-1] << " "
		     << (int) sum0[w/2] << " "
		     << (int) sum0[w/2+1] << " ... "	
		     << (int) sum0[w-3] << " "
		     << (int) sum0[w-2] << " "
		     << (int) sum0[w-1] << " "
		     << Log::dp;
  
  return true;
}

/**
 * Calculate the row/col sums of the whole image
 */
bool ImageProcessor::calculateSums(NDArray *pArray) {
  epicsUInt32 w = pArray->dims[0].size; // X dimension

  epicsUInt16 *rowPtr = (epicsUInt16 *) pArray->pData;
  _imageRow.assign(rowPtr, rowPtr + w);
 
  _sumX.resize(w);
  _sumY.resize(w);

  std::fill(_sumX.begin(), _sumX.end(), 0);
  std::fill(_sumY.begin(), _sumY.end(), 0);

  //  std::vector<double>::iterator itHistY = _sumY.begin();

  for (unsigned int i = 0; i < pArray->dims[1].size; ++i) {
    _imageRow.assign(rowPtr, rowPtr + w);
    std::transform(_sumX.begin(), _sumX.end(),
		   _imageRow.begin(), _sumX.begin(), std::plus<long>());

    //*itHistY = std::accumulate(_imageRow.begin(), _imageRow.end(), 0);
    _sumY[i] = std::accumulate(_imageRow.begin(), _imageRow.end(), 0);

    rowPtr += w;
  }
  
  return true;
}

/**
 * Sum the pixel intensities for a given column.
 */
unsigned long ImageProcessor::sumColumn(NDArray *pArray, unsigned int columnIndex) {
  epicsUInt32 w = pArray->dims[0].size; // X dimension
  epicsUInt16 *colPtr = (epicsUInt16 *) pArray->pData;
  unsigned long intensity = 0;

  /** Can't sum a column that is outside the image boundaries */
  if (columnIndex >= pArray->dims[1].size) {
    return 0;
  }

  colPtr += columnIndex;
  for (epicsUInt32 row = 0; row < w; ++row) {
    intensity += *colPtr;
    colPtr += w;
  }

  return intensity;
}

/**
 * Sum the pixel intensities for a given row.
 */
unsigned long ImageProcessor::sumRow(NDArray *pArray, unsigned int rowIndex) {
  epicsUInt32 w = pArray->dims[0].size; // X dimension
  epicsUInt32 h = pArray->dims[1].size; // Y dimension
  epicsUInt16 *rowPtr = (epicsUInt16 *) pArray->pData;
  unsigned long intensity = 0;

  /** Can't sum a row that is outside the image boundaries */
  if (rowIndex >= pArray->dims[0].size) {
    return 0;
  }

  rowPtr += (w * rowIndex); // skip rows before it

  _imageRow.assign(rowPtr, rowPtr + w);
 
  for (epicsUInt32 col = 0; col < h; ++col) {
    intensity += rowPtr[col];
  }

  return intensity;
}

bool ImageProcessor::refinePoint(NDArray *pArray, unsigned long bgThreshold,
				 Point &pixel, unsigned int scanInterval,
				 int size, bool isBefore, bool isColumn) {
  bool found = false;
  unsigned int pixelIndex = pixel.x;
  if (!isColumn) {
    pixelIndex = pixel.y;
  }

  unsigned int interval = scanInterval;
  unsigned long intensity = 0;
  for (int i = 1; i < 4 && !found; ++i) {
    interval /= 2;
    if (isBefore) {
      pixelIndex -= interval;
      if (pixelIndex < 0) {
	pixelIndex = 0;
      }
    }
    else {
      pixelIndex += interval;
    }
    if (isColumn) {
      intensity = sumColumn(pArray, pixelIndex);
    }
    else {
      intensity = sumRow(pArray, pixelIndex);
    }

    if (intensity < bgThreshold * size) {
      found = true;
      pixel.x = pixelIndex;
      pixel.y = pixelIndex;
      pixel.intensity = intensity;
    }

    Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpInfo
		       << "INFO: pixelIndex=" <<(int) pixelIndex
		       << " inc=" << (int)interval
		       << " int=" << (int)intensity << Log::dp;
  }

  if (!found) {
    if (isBefore) {
      pixel.x -= scanInterval;
      pixel.y -= scanInterval;
      if (pixel.x < 0) {
	pixel.x = 0;
	pixel.y = 0;
      }
    }
    else {
      pixel.x += scanInterval;
      pixel.y += scanInterval;
      if (pixel.y > size) {
	pixel.y = size - 1;
      }
      if (pixel.x > size) {
	pixel.x = size - 1;
      }
    }
    if (isColumn) {
      pixel.intensity = sumColumn(pArray, pixel.x);
    }
    else {
      pixel.intensity = sumRow(pArray, pixel.y);
    }
  }

  Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpInfo
		     << "INFO: found at [" << (int) pixel.x
		     << ", " << (int) pixel.y << "]"
		     << " inc=" << (int)interval
		     << " int=" << (int)pixel.intensity << Log::dp;

  if (pixel.x < 0 || pixel.y < 0) {
    Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpError
		       << "ERROR: Failed to refine before point, giving up (x="
		       << pixel.x << ", y=" << pixel.y << ")"
		       << Log::dp;
    return false;
  }

  return true;
}


bool ImageProcessor::findROIPoints(NDArray *pArray, unsigned int scanInterval,
				   unsigned long bgThreshold, Point &before, Point &after,
				   bool isColumn, unsigned long *averageIntesityFound) {
  _roiW = 0;
  _roiH = 0;

  *averageIntesityFound = 0;
  unsigned int avgCount = 0;

  epicsUInt32 size = 0;
  if (isColumn) {
    size = pArray->dims[0].size; // X dimension
  }
  else {
    size = pArray->dims[1].size; // Y dimension
  }

  before.x = -1;
  before.y = -1;
  before.intensity = 0;
  after.x = -1;
  after.y = -1;
  after.intensity = 0;

  int numScans = size / scanInterval;

  // Define in which 
  unsigned int start;
  if (numScans % 2 == 0) {
    start = (size % scanInterval) / 2;
  }
  else {
    start = size / 2 - (numScans/2) * scanInterval;
  }

  bool foundLeft = false;
  unsigned int pixelIndex = start;
  unsigned long intensity = 0;

  Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpInfo
		     << "INFO: Scanning starting at pixel " << (int) start
		     << ", num scans=" << numScans<< Log::dp;

  for (int i = 0; i < numScans; ++i, ++avgCount) {
    if (isColumn) {
      intensity = sumColumn(pArray, pixelIndex);
    }
    else {
      intensity = sumRow(pArray, pixelIndex);
    }

    *averageIntesityFound += intensity / size;

    Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpInfo
		       << "INFO: Scanning at pixel " << (int) pixelIndex
		       << ", interval=" << (int) scanInterval
		       << ", intensity=" << (int) intensity
		       << ", thres=" << (int) (size * bgThreshold)<< Log::dp;

    if (intensity > bgThreshold * size && !foundLeft) {
      before.x = pixelIndex;
      before.y = pixelIndex;
      before.intensity = intensity;

      after.x = before.x;
      after.y = before.y;
      after.intensity = before.intensity;

      foundLeft = true;
    }
    else {
      if (intensity > bgThreshold * size) {
	after.x = pixelIndex;
	after.y = pixelIndex;
	after.intensity = intensity;
      }
    }

    pixelIndex += scanInterval;
  }

  *averageIntesityFound /= avgCount;
  
  if (!foundLeft) {
    Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpError
		       << "ERROR: Failed to find before point, giving up."
		       << Log::dp;
    return false;
  }

  Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpInfo
		     << "INFO: Before pixel at " << (int) before.x << ", after pixel at " << (int) after.x << Log::dp;

  foundLeft = refinePoint(pArray, bgThreshold, before, scanInterval, size, true, isColumn);

  if (!foundLeft) {
    Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpError
		       << "ERROR: Failed to refine before point, giving up."
		       << Log::dp;
    return false;
  }

  // Refine right - must be between after.x and
  // (after.x + scanInterval or w)
  bool foundRight = false;

  foundRight = refinePoint(pArray, bgThreshold, after, scanInterval, size, false, isColumn);

  if (before.x == after.x) {
    return false;
  }

  if (isColumn) {
    _topROI.x = before.x;
    _bottomROI.x = after.x;
  }
  else {
    _topROI.y = before.y;
    _bottomROI.y = after.y;
  }
  
  _roiW = _bottomROI.x - _topROI.x + 1;
  _roiH = _bottomROI.y - _topROI.y + 1;

  if (_roiW < 0 || _roiH < 0) {
    Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpWarn
		       << "WARN: found W/H swaller than 0 (w="
		       << _roiW << ", h=" << _roiH << ")" << Log::dp;
    Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpWarn
		       << "WARN: top[" << _topROI.x << "," << _topROI.y
		       << "]; bottom[" << _bottomROI.x << "," << _bottomROI.y << "]"
		       << Log::dp;
    _roiW = 0;
    _roiH = 0;
  }

  if (before.x < 0 || after.x < 0) {
    Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpError
		       << "ERROR: Invalid points before=" << before.x
		       << ", after=" << after.x
		       << Log::dp;
    return false;
  }

  if (before.x > after.x) {
    Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpError
		       << "ERROR: Invalid points (before>after) found before=" << before.x
		       << ", after=" << after.x
		       << Log::dp;
    return false;
  }

  return true;
}

void ImageProcessor::getROISize(int &w, int &h) {
  w = _roiW;
  h = _roiH;
}

Waveform &ImageProcessor::getROIProfile() {
  return _roiProjection;
}

Waveform &ImageProcessor::getROISliceProfile(unsigned short sliceNumber) {
  if (sliceNumber >= _roiSlices) {
    sliceNumber = 0;
  }
  return _roiVerticalProjection[sliceNumber];
}

WaveformDouble &ImageProcessor::getROIEnergy() {
  return _energy;
}

/**
 * De
 */
// bool ImageProcessor::findReferenceROI() {
// }

/**
 * Finds the upper left and bottom right points for the ROI cointaining beam.
 * The ROI search is based on the row/column pixel intensity sum. The image is scanned
 * on X and Y at half of the expected ROI size (expectedROIX and expectedROIY).
 *
 * @param pArray full image
 * @param bgThreshold background pixel intensity - defines whether a row/column has
 * beam or not
 * @expectedROIX expected size in X of the ROI containing beam
 * @expectedROIY expected size in Y of the ROI containing beam
 *
 * @return true if an ROI was found, false if the algorithm could not find the beam
 * area
 */
bool ImageProcessor::findROI(NDArray *pArray, long bgThreshold,
			     unsigned int expectedROIX, unsigned int expectedROIY) {
  Point scanXLeft;
  Point scanXRight;
  Point scanYTop;
  Point scanYBottom;

  unsigned int scanIntervalX = expectedROIX / 2;
  unsigned int scanIntervalY = expectedROIY / 2;
  
  bool isColumn;

  int tentative = 1;
  bool status = false;
  /** First find the left and right ROI X coordinate points */
  isColumn = true; // when finding in X the intensity is added up across a column

  unsigned long threshold = bgThreshold;

  unsigned long averageIntensityFound = 0;

  while (!status && tentative < 7) {
    status = findROIPoints(pArray, scanIntervalX, threshold,
			   scanXLeft, scanXRight, isColumn,
			   &averageIntensityFound);
    if (!status) {
      _roiErrorStatus = ROI_FAIL_CROP;
    }
    //    threshold = bgThreshold + 2 * tentative;

    // If can't find right, threshold is too high. Use the own image
    // sample background as threshold
    if (scanXLeft.x < 0) {
      threshold = /*averageIntensityFound*/bgThreshold + 2 * tentative;
      Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpWarn
			 << "WARN: ROI not found (W)"
			 << ". Trying again with average threshold found ["
			 << tentative << "] " << threshold
			 << Log::dp;
      status = false;
    }
    else {
      unsigned int roiSize = scanXRight.x - scanXLeft.x;
      if (roiSize > expectedROIX) {
	if (averageIntensityFound >= threshold) {
	  threshold = averageIntensityFound + 2 * tentative;
	}
	else {
	  threshold = bgThreshold + 2 * tentative;
	}
	Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpWarn
			   << "WARN: ROI width too large "
			   << scanXLeft.x << " - " << scanXRight.x 
			   << ", expected " << expectedROIX
			   << ". Trying again with increased threshold ["
			   << tentative << "] " << threshold
			   << ". Average int found="
			   << averageIntensityFound
			   << Log::dp;
	status = false;
      }
    }
    tentative++;
  }
  if (!status) {
    return false;
  }

  /** Next find the top and bottom ROI Y coordinate points */
  tentative = 1;
  status = false;
  isColumn = false;
  threshold = bgThreshold;
  while (!status && tentative < 7) {
    status = findROIPoints(pArray, scanIntervalY, threshold,
			   scanYTop, scanYBottom, isColumn,
			   &averageIntensityFound);
    if (!status) {
      _roiErrorStatus = ROI_FAIL_CROP;
    }

    //    threshold = bgThreshold + 2 * tentative;
    // If can't find right, threshold is too high. Use the own image
    // sample background as threshold
    if (scanYTop.y < 0) {
      threshold = /*averageIntensityFound*/bgThreshold + 2 * tentative;
      Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpWarn
			 << "WARN: ROI not found (H)"
			 << ". Trying again with average threshold found ["
			 << tentative << "] " << threshold
			 << Log::dp;
      status = false;
    }
    else {
      unsigned int roiSize = scanYBottom.y - scanYTop.y;
      if (roiSize > expectedROIY) {
	if (averageIntensityFound >= threshold) {
	  threshold = averageIntensityFound + 2 * tentative;
	}
	else {
	  threshold = bgThreshold + 2 * tentative;
	}
	Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpWarn
			   << "WARN: ROI height too large "
			   << scanYTop.y << " - " << scanYBottom.y
			   << ", expected " << expectedROIY
			   << ". Trying again with increased threshold ["
			   << tentative << "] " << threshold
			   << ". Average int found="
			   << averageIntensityFound
			   << Log::dp;
	status = false;
	threshold = bgThreshold + 2 * tentative;
      }
    }
    tentative++;
  }
  if (!status) {
    return false;
  }

    /*

  if (findROIPoints(pArray, scanIntervalY, bgThreshold, scanYTop, scanYBottom, isColumn) == false) {
    _roiErrorStatus = ROI_FAIL_CROP;
    return false;
  }

  if (scanYBottom.y - scanYTop.y > expectedROIY) {
    Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpWarn
		       << "WARN: ROI height too large "
		       << scanYTop.y << " - " << scanYBottom.y 
		       << ", expected " << expectedROIY
		       << ". Trying again with higher threshold."
		       << Log::dp;
    if (findROIPoints(pArray, scanIntervalY, bgThreshold * 1.1, scanYTop, scanYBottom, isColumn) == false) {
      _roiErrorStatus = ROI_FAIL_CROP;
      return false;
    }
  }
    */

  _topROI.x = scanXLeft.x;
  _topROI.y = scanYTop.y;

  _bottomROI.x = scanXRight.x;
  _bottomROI.y = scanYBottom.y;

  Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpInfo
		     << "INFO: top left [" << (unsigned int) _topROI.x
		     << ", " << (unsigned int) _topROI.y
		     << "] bottom right [" << (unsigned int) _bottomROI.x
		     << ", " << (unsigned	int) _bottomROI.y
		     << "]" << Log::dp;

  _roiErrorStatus = ROI_OK;
  return true;
}

/**
 * This method is called when a background image is collected.
 */
bool ImageProcessor::saveBackground(NDArray *pArray) {
  Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpInfo
		     << "INFO: Saving background image." << Log::dp;

  _backgroundArray.ndims = 2;

  _backgroundArray.dims[0].size = pArray->dims[0].size;
  _backgroundArray.dims[0].offset = 0;
  _backgroundArray.dims[0].binning = 0;
  _backgroundArray.dims[0].reverse = 0;

  _backgroundArray.dims[1].size = pArray->dims[1].size;
  _backgroundArray.dims[1].offset = 0;
  _backgroundArray.dims[1].binning = 0;
  _backgroundArray.dims[1].reverse = 0;

  _backgroundArray.dataType = NDUInt16;
  _backgroundArray.dataSize = _backgroundArray.dims[0].size *
    _backgroundArray.dims[1].size * 4;
  _backgroundArray.pData = realloc(_backgroundArray.pData, _backgroundArray.dataSize);
  
  memcpy(_backgroundArray.pData, pArray->pData, _backgroundArray.dataSize);

  if (saveBackgroundThreshold() == false) {
    return false;
  }

  return true;
}

void ImageProcessor::getROIPoints(Point &topLeft, Point &bottomRight) {
  topLeft = _topROI;
  bottomRight = _bottomROI;
}

/**
 * Subtract the background image from the ROI region. Both ROI and Background 
 * images must be available. The background starting at the _topROI point
 * is used.
 */
bool ImageProcessor::subtractBackground() {
  if (_backgroundArray.ndims != 2) {
    Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpError
		       << "ERROR: Background image not available for subtraction"
		       << Log::dp;
    return false;
  }

  if (_roiArray.ndims != 2) {
    Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpError
		       << "ERROR: ROI image not available for background subtraction"
		       << Log::dp;
    return false;
  }

  unsigned int size = _topROI.x + _roiArray.dims[0].size;
  if (size > _backgroundArray.dims[0].size) {
    Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpError
		       << "ERROR: ROI X image dimensions do not fit into background"
		       << Log::dp;
    return false;
  }

  size = _topROI.y + _roiArray.dims[1].size;
  if (size > _backgroundArray.dims[1].size) {
    Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpError
		       << "ERROR: ROI Y image dimensions do not fit into background"
		       << Log::dp;
    return false;
  }

  epicsUInt32 backgroundW = _backgroundArray.dims[0].size; // X dimension
  epicsUInt32 roiW = _roiArray.dims[0].size; // X dimension

  epicsUInt16 *rowPtr = (epicsUInt16 *) _backgroundArray.pData;
  rowPtr += _topROI.y * backgroundW;
  rowPtr += _topROI.x;

  epicsUInt16 *roiRowPtr = (epicsUInt16 *) _roiArray.pData;

  for (unsigned int i = 0; i < _roiArray.dims[1].size; ++i) {
    _imageRow.assign(rowPtr, rowPtr + roiW);
    _roiRow.assign(roiRowPtr, roiRowPtr + roiW);

    if (i == 0) {
      Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpInfo
			 << "INFO: pixel at roi [0,0] intensity "
			 << roiRowPtr[0] << ", background "
			 << rowPtr[0] << " " << _roiW
			 << Log::dp;
    }

//     std::transform(_roiRow.begin(), _roiRow.end(),
//  		   _imageRow.begin(), _roiRow.begin(), std::minus<epicsUInt16>());

    for (unsigned int j = 0; j < roiW; ++j) {
      if (rowPtr[j] > roiRowPtr[j]) {
	roiRowPtr[j] = 0;
      }
      else {
	roiRowPtr[j] -= rowPtr[j];
      }
    }

    if (i == 0) {
      Log::getInstance() << Log::showtime << Log::flagBackground << Log::dpInfo
			 << "INFO: pixel at roi [0,0] intensity "
			 << roiRowPtr[0]
			 << Log::dp;
    }

    rowPtr += backgroundW;
    roiRowPtr += roiW;
  }

  return true;
}

bool ImageProcessor::saveROI(NDArray *pArray) {
  if (pArray->ndims != 2) {
    return false;
  }

  epicsUInt16 *srcPtr = (epicsUInt16 *) pArray->pData;
  epicsUInt16 *destPtr = (epicsUInt16 *) _roiArray.pData;

  _roiArray.dims[0].size = _bottomROI.x - _topROI.x + 1;
  _roiArray.dims[1].size = _bottomROI.y - _topROI.y + 1;
  
  srcPtr += _topROI.y * pArray->dims[0].size; // Skip first rows
  srcPtr += _topROI.x; // Skip first cols

  // Copy row by row
  for (unsigned int i = 0; i < _roiArray.dims[1].size; ++i) {
    memcpy(destPtr, srcPtr, _roiArray.dims[0].size * sizeof(epicsUInt16));

    if (i < 4) {
      int w = (_bottomROI.x - _topROI.x + 1);
      Log::getInstance() << Log::showtime << Log::flagCrop << Log::dpInfo
			 << "INFO: ROI row " << i << ": "
			 << (unsigned int) destPtr[0] << " "
			 << (unsigned int) destPtr[1] << " "
			 << (unsigned int) destPtr[2] << " ... "
			 << (unsigned int) destPtr[w/2-1] << " "
			 << (unsigned int) destPtr[w/2] << " "
			 << (unsigned int) destPtr[w/2+1] << " ... "	
			 << (unsigned int) destPtr[w-3] << " "
			 << (unsigned int) destPtr[w-2] << " "
			 << (unsigned int) destPtr[w-1] << " at address "
			 << (unsigned long) destPtr
			 << Log::dp;
    }
    destPtr += _roiArray.dims[0].size;
    srcPtr += pArray->dims[0].size;
  }
  return true;
}

NDArray *ImageProcessor::getROIArray() {
  return &_roiArray;
}

void ImageProcessor::setCharge(double charge) {
  _charge = charge;
}

void ImageProcessor::setStrengthParam(double strength) {
  Log::getInstance() << Log::showtime << Log::flagEnergy << Log::dpInfo
		     << "INFO: Setting strength parameter to " << strength
		     << Log::dp;
  _strengthParam = strength;
};

void ImageProcessor::setROISlices(unsigned short roiSlices) {
  _roiSlices = roiSlices;

  _roiProjection.resize(_roiSlices);
  _sliceSize.resize(_roiSlices);
  _energy.resize(_roiSlices);
  _roiVerticalProjection.resize(_roiSlices);

  for (Waveforms::iterator it = _roiVerticalProjection.begin();
       it != _roiVerticalProjection.end(); ++it) {
    (*it).reserve(MAX_ROI_H);
    (*it).resize(MAX_ROI_H/2);
    std::fill((*it).begin(), (*it).end(), 0);
  }
}

/**
 * Calculate the energy of each ROI slice
 */
bool ImageProcessor::calculateEnergy() {
  std::fill(_energy.begin(), _energy.end(), 0);
  double fudge = 0;
  for (int i = 0; i < _roiSlices; ++i) {
    double delta_t = (1 / 0.3) *(1 / _strengthParam) * _sliceSize[i];
    fudge += _roiProjection[i] * delta_t;
  }
  fudge /= _charge;

  for (int i = 0; i < _roiSlices; ++i) {
    _energy[i] = _roiProjection[i] / fudge;
    Log::getInstance() << Log::showtime << Log::flagEnergy << Log::dpInfo
		       << "INFO: fudge=" << fudge
		       << " energy=" << _energy[i]
		       << " charge=" << _charge
		       << " sum=" << _roiProjection[i]
		       << " s=" << _strengthParam
		       << " pixels=" << _sliceSize[i]
		       << Log::dp;
  }

  return true;
}

/**
 * Produce the horz/vert projections on the ROI based 
 * on the number of slices set.
 */
bool ImageProcessor::sliceROI() {
  epicsUInt16 *rowPtr = (epicsUInt16 *) _roiArray.pData;

  unsigned short sliceSize = _roiArray.dims[0].size / _roiSlices;
  unsigned short extraCount = 0;
  unsigned short extras = 0;

  if (_roiSlices > _roiArray.dims[0].size) {
    Log::getInstance() << Log::showtime << Log::flagSlice << Log::dpInfo
		       << "INFO: Image has less columns ("
		       << _roiArray.dims[0].size << ") than the number"
		       << " of desired slices (" << _roiSlices
		       << ")." << Log::dp;
    _roiSlices = _roiArray.dims[0].size;
  }

  // Spread the remaining columns starting at the first slice
  unsigned short remainder = _roiArray.dims[0].size % _roiSlices;

  /*
  // If the last remaining slice is larger than twice the regular slice
  // then spread the extra columns between the first slices
  unsigned short remainder = _roiArray.dims[0].size % _roiSlices;
  if ((remainder > sliceSize * 2) && (remainder < _roiSlices)) {
    extras = _roiArray.dims[0].size % _roiSlices;
    Log::getInstance() << Log::showtime << Log::flagSlice << Log::dpInfo
		       << "INFO: Distributing extra slices, last would receive "
		       << _roiArray.dims[0].size % _roiSlices << ", when the "
		       << "slice size is " << sliceSize << "."
		       << Log::dp;
  }
  */

  Log::getInstance() << Log::showtime << Log::flagSlice << Log::dpInfo
		     << "INFO: sliceSize=" << (int) sliceSize
		     << ", _roiSlices=" << _roiSlices
		     << ", _roiArray.dims[0].size=" << _roiArray.dims[0].size
		     << Log::dp;

  std::fill(_roiProjection.begin(), _roiProjection.end(), 0);
  std::fill(_sliceSize.begin(), _sliceSize.end(), 0);

  for (int i = 0; i < _roiSlices; ++i) {
    std::fill(_roiVerticalProjection[i].begin(), _roiVerticalProjection[i].end(), 0);
  }

  // Go through each ROI row
  for (unsigned int i = 0; i < _roiArray.dims[1].size; ++i) {
    // Then for each slice within a row
    for (int j = 0; j < _roiSlices; ++j) {
      sliceSize = _roiArray.dims[0].size / _roiSlices;
      if (extraCount > 0) {
	sliceSize += 1;
	extraCount -= 1;
      }
      // Add extra pixels to the last slice
      if (j == _roiSlices - 1) {
	if (_roiArray.dims[0].size % _roiSlices != 0) {
	  sliceSize += _roiArray.dims[0].size % _roiSlices;
	  sliceSize -= extras; // take the extra slices already used
	  if (i == 0) {
	    Log::getInstance() << Log::showtime << Log::flagSlice << Log::dpInfo
			       << "INFO: last slice with more cols=" << sliceSize
			       << Log::dp;
	  }
	}
      }

      // Save the number of pixel used for each slice
      _sliceSize[j] = sliceSize;

      _imageRow.assign(rowPtr, rowPtr + sliceSize);

      unsigned long rowSum = 0;
      rowSum = std::accumulate(_imageRow.begin(), _imageRow.end(), rowSum);

      if (i == 1 && j == 0) {
	Log::getInstance() << Log::showtime << Log::flagSlice << Log::dpInfo
			   << "INFO: slice row sum=" << rowSum
			   << " i=" << i << " j=" << j
			   << " sliceSize=" << sliceSize
			   << Log::dp;
      }

      if (i == 1 && j == _roiSlices - 1) {
	Log::getInstance() << Log::showtime << Log::flagSlice << Log::dpInfo
			   << "INFO: slice row sum=" << rowSum
			   << " i=" << i << " j=" << j
			   << " sliceSize=" << sliceSize
			   << Log::dp;
      }
      
      try {
	// Save slice vertical projection
	_roiVerticalProjection[j].at(i) = rowSum;
	
	// Accumulate slice sum into horz projection
	_roiProjection[j] += rowSum;
      } catch (std::out_of_range &e) {
	Log::getInstance() << Log::showtime << Log::flagSlice << Log::dpError
			   << "ERROR: std::out_of_range exception!" << Log::dp;
	return false;
      }

      rowPtr += sliceSize;
    }
  }

  Log::getInstance() << Log::showtime << Log::flagSlice << Log::dpInfo
		     << "INFO: cols per slice: ";
  for (int j = 0; j < _roiSlices; ++j) {
    Log::getInstance() << Log::flagSlice << Log::dpInfo
		       << _sliceSize[j] << " ";
  }
  Log::getInstance() << Log::dp;


  return true;
}