/*
* Copyright (c) 2009 Stanford University, School of Medicine All rights reserved. 
* 
* STANFORD ACADEMIC SOFTWARE LICENSE AGREEMENT 
* FOR AngioLocalize SOURCE CODE: 
* 
* 1.  This is a legal agreement between you, RECIPIENT, and STANFORD UNIVERSITY.  
*       By accepting, receiving, and using AngioLocalize.including any accompanying information, materials or 
*       manuals (Program), you are agreeing to be bound by the terms of this Agreement.  
*       If you do not agree to the terms of this Agreement, promptly return the Program to the STANFORD.

* 2.  STANFORD grants to RECIPIENT a royalty-free, nonexclusive, and nontransferable license to use the Program 
*       and accompanying source code furnished hereunder, upon the terms and conditions set out below. 
*       documentation and/or other materials provided with the distribution. 
* 3.  RECIPIENT acknowledges that the Program is a research tool still in the development stage and that 
*       it is being supplied as is, without any accompanying services, support or improvements from STANFORD.
* 4.  RECIPIENT agrees to use the Program solely for internal non-commercial purposes. 
* 5.  RECIPIENT may create derivatives and further transfer the Program under the following terms and conditions: 
*        A) Recipient agrees to not commercially exploit the Program and agrees all uses will be for non-commercial purposes; and,
*        B) Recipient may not mark any derivatives with the title of Program and may not to use iPad mark for any derivatives; and,
*        C) Recipient agrees to mark and acknowledge all derivatives as follows: "This is a derivative work based on AngioLocalize:
*           Frameless AVM localization with 2D Digital Subtraction Angiography (DSA) and 3D Rotational Angiography (DRA) Images developed
*           by Dimitre Hristov, Lina Liu.
*        D) Recipient agrees to make all improvements, enhancements and derivatives available under 
*           similar terms and conditions to the research community. 
* 6.   RECIPIENT warrants that RECIPIENT will not remove or export any part of the software or Program from the United 
*       States except in full compliance with all United States and other applicable laws and regulations.
* 7.  RECIPIENT will use the Program in compliance with all applicable laws, policies and regulations including, but not 
*       limited to, any approvals, informed consent and patient confidentiality principles.
* 8.   RECIPIENT will indemnify, hold harmless, and defend Stanford against any claim of any kind arising out of or
*       related to the exercise of any rights granted under this Agreement or the breach of this Agreement by RECIPIENT.
* 9.  Title and copyright to the Program and any associated documentation shall at all times remain with STANFORD, 
*       and RECIPIENT agrees to preserve same.
*/ 
//----------------------------------------------------------------------------------
//! The ML module class ChamferMatching.
/*!
// \file    mlChamferMatching.cpp
// \author  lina liu
// \date    2008-08-28
//
// The 2DChamferMatching module matches a contour given as a XMarkerList to a distance 
// 2D image by iteratively minimizing the sum of the (interpolated) pixel values at the marker positions.
*/
//----------------------------------------------------------------------------------

// Local includes
#include "mlChamferMatching.h"

ML_START_NAMESPACE


//! Implements code for the runtime type system of the ML
ML_BASEOP_CLASS_SOURCE(ChamferMatching, BaseOp);

//----------------------------------------------------------------------------------
//! Constructor
//----------------------------------------------------------------------------------
ChamferMatching::ChamferMatching ()
  : BaseOp(2, 0)
{
  ML_TRACE_IN("ChamferMatching::ChamferMatching ()");

  // Suppress calls of handleNotification on field changes to
  // avoid side effects during initialization phase.
  handleNotificationOff();

   const char * funcNames[] = {
      "Amoeba",                //0
	  "Amebsa"
   };


  //! Get reference to the container for parameters/fields.
  FieldContainer &fields = *getFieldContainer();

  ML_CHECK(&fields);
  
  
  _modeFld = fields.addEnum ("algorithms", funcNames, numAlgorithms);
  _modeFld->setEnumValue(amoeba);

  // Initializes loaded image buffer.
  _inputImage.setData(NULL);
  _inputImage.setBox(SubImgBox());

  // add all the fields in UI
  _matrix1Fld = fields.addMatrix("matrix1");
  _matrix2Fld = fields.addMatrix("matrix2");
  _matrix3Fld = fields.addMatrix("matrix3");

  //! initalize the matrixtoUse variable
  float mat[16];
  for (int i = 0; i< 16; i++) {
	  mat[i] = (i%5 == 0)? 1 : 0;  
  }
  
  _matrixToUse.setValues(mat);

  _apply1Fld = fields.addBool("apply1");
  _apply1Fld->setBoolValue(true);
  _apply2Fld = fields.addBool("apply2");
  _apply2Fld->setBoolValue(true);
  _apply3Fld = fields.addBool("apply3");
  _apply3Fld->setBoolValue(true);

  _startFld = fields.addNotify("start");
  _stopFld = fields.addNotify("stop");
  _continueFld = fields.addNotify("continue");

  _updateFreqFld = fields.addInt("updateFreq");
  _accuracyFld = fields.addFloat("accuracy");
  _iterationsFld = fields.addInt("iterations");
  _temperatureFld = fields.addFloat("Temperature"); 
  _annealingFld = fields.addFloat("AnnealingConstant"); 
  _finalCostFld = fields.addFloat("finalCost"); 
  _numMarkersFld = fields.addInt("numMarkers");

  (_inMarkerListFld = fields.addBase("inMarkerList"))->setBaseValue(NULL);

  _inMarkerList = NULL;
  _outMarkerList = NULL;
  _filteredMarkerList = NULL;
  _hasMaskImage = false;
  _input1Img = NULL;
  _maskImg = NULL;

  // Connect input fields with output field to update connected
  // modules if input images change.
  for (int inIdx=0; inIdx < 0; ++inIdx){
    for (int outIdx=0; outIdx < 0; ++outIdx){
      getInField(inIdx)->attachField(getOutField(outIdx));
    }
  }
  
  handleNotificationOn();
}


//----------------------------------------------------------------------------------
//! this function is used in the test function to test the amoeba algorithm.
//! this function is written as global because the signature of amoeba requires
//! global function.
//----------------------------------------------------------------------------------
/*double func(VecDoub &x, void *obj)
{
	ChamferMatching *myObj = (ChamferMatching *)obj;
    return SQR(x[0]) + SQR(x[1]);
}*/



//----------------------------------------------------------------------------------
//! updateFunc function that update GUI
//----------------------------------------------------------------------------------
void updateFunc(void *obj)
{
  ML_TRACE_IN("updateFunc")
  //get the projected XMarkerList

  ChamferMatching* curr = (ChamferMatching*) obj;
  curr->updateMatrix1Fld();
  curr->updateMatrix2Fld();
 // curr->updateMatrix3Fld();

}



//----------------------------------------------------------------------------------
//! cost function that calculates the cost
//----------------------------------------------------------------------------------
double cost(VecDoub &x, void *obj)
{
  ML_TRACE_IN("cost")
 
  ChamferMatching* curr = (ChamferMatching*) obj;
  
  curr->generateMatrices(x);
  curr->doTransform();

  XMarkerList *outMarkerList = curr->_outMarkerList;

  double sum = 0.0;
  int IntMarkerListSize=outMarkerList->getSize();
	  for(size_t i = 0; i<IntMarkerListSize; i++) {
		   XMarker *marker = (XMarker*)outMarkerList->getItemAt(i);
		   double *voxelVal = NULL;
		   if (curr->getVoxelValue(0,marker, curr->_input1Img, &voxelVal))
			   sum += *voxelVal;
		   else sum+=(curr->_input1Img)->getMaxVoxelValue(); //
	  }
	  return sum/IntMarkerListSize;
}

//----------------------------------------------------------------------------------
//! test function to test ameoba function
//----------------------------------------------------------------------------------

Doub test(VecDoub &x)
{
	return SQR(x[0]) + SQR(x[1]);
}

bool isStopButtonPressed(void *obj) 
{ 
  Field *field = NULL; 
  ChamferMatching* curr = (ChamferMatching*) obj;

 /* do { 
	  field = curr->getPressedNotifyField(); 
	 // if (curr->getStopFld() == *field)  
          { 
            return true; 
          } 
  } while (field); 
 
  return false; */
  return true;
} 


//----------------------------------------------------------------------------------
//! getVoxelValue from a marker on a image
//----------------------------------------------------------------------------------
bool ChamferMatching::getVoxelValue(MLint OutIndex, XMarker  *marker, PagedImg *inImg, double ** voxelValue) {

	vec6 pos = marker->pos;
	float x = pos[0];
	float y = pos[1];
	float z = pos[2];

	inImg->transformToVoxelCoord(&x, &y, &z);
	Vector imgExt = inImg->getImgExt();
	if (imgExt.x <= x
         || x < 0
         || imgExt.y <= y
         || y < 0
         || imgExt.z <= z
         || z < 0
         || imgExt.c <= pos[3]
         || pos[3] < 0
         || imgExt.t <= pos[4]
         || pos[4] < 0
         || imgExt.u <= pos[5]
         || pos[5] < 0
         )
      {
		  // image out of boundary
		  return false; 
		 }
		// create subimage box consisting only of the voxel
    SubImgBox voxelBox = SubImgBox(Vector((MLint)x, (MLint)y, (MLint)z,
                                              (MLint)(pos[3]),
                                              (MLint)(pos[4]),
                                              (MLint)(pos[5])),
                                       Vector((MLint)x, (MLint)y, (MLint)z,
                                              (MLint)(pos[3]),
                                              (MLint)(pos[4]),
                                              (MLint)(pos[5])));

        // get voxel value
    getTile(getInOp(OutIndex), getInOpIndex(OutIndex),voxelBox, MLdoubleType, (void **)(voxelValue));	
    return true;
}



//----------------------------------------------------------------------------------
//! Handle field changes of the field field.
//----------------------------------------------------------------------------------
void ChamferMatching::handleNotification (Field *field)
{
  ML_TRACE_IN("ChamferMatching::handleNotification ()");


  if (field == _inMarkerListFld) {

      if (ML_BASE_IS_A((static_cast<Base*>(_inMarkerListFld->getBaseValue())), XMarkerList)){

		  _inMarkerList = static_cast<XMarkerList*>(_inMarkerListFld->getBaseValue());

		  if (_inMarkerList != NULL){
			/*if (!_isInNotificationCB) { 
				_inputList->addNotificationObserver(_csoListNotificationCB, this); 
			}*/
		  }
		} else {
		  _inMarkerList = NULL;
		}
  }

  if (field == _modeFld) {
      if (_modeFld->getEnumValue() < 0 || _modeFld->getEnumValue() >= numAlgorithms) {
		  ML_PRINT_ERROR("ChamferMatching:getAlgorithms", ML_BAD_PARAMETER,
                       "Invalid alogithm parameter passed, using 'Ameoba' instead.");
      }

  }
  
  int mode = _modeFld->getEnumValue();
  if (mode < 0 || mode >= numAlgorithms) {
      mode = amoeba;
    }

  if (getUpdatedInImg(0) == NULL) {
      // input image is invalid
      return;
  } else {
	  _input1Img = getInImg(0);
  }

  if (getUpdatedInImg(1)) {
	  _maskImg = getInImg(1); 
	  _hasMaskImage = true;
  } else {
	  _hasMaskImage = false;
  }

  

  if (field == _startFld) {
	  if (_inMarkerList == NULL){
		  return;
	  }
	  if (_filteredMarkerList) delete _filteredMarkerList;
	  if (_outMarkerList) delete _outMarkerList;

	  getValuesFromUI();	
	  filter();
	  int numOfIter = 0;
	  VecDoub x_initial(_elementNum,0.0); // initialize the transformation vector
	  VecDoub x(_elementNum,0.0); // initialize the transformation vector
	  
	  if (mode == amoeba ) {
		//! call ameoba minimize function

		//minimum = am.minimize(x, accuracy, cost, updateFunc, isStopButtonPressed, this);
		Amoeba am(_accuracy, _iterations, _updateFreq); //initialize the minimization
		x=am.minimize(x_initial, _DEL, cost, updateFunc, isStopButtonPressed, this);
		numOfIter = am.nfunc;

	   } else {

		  //! call simulated annealing function here

		Amebsa<Doub(VecDoub &, void*)> am(x_initial, _DEL, cost, _accuracy, this);

		int iter = _iterations/_updateFreq; //initial number of iterations
		double temp=_temperature; // initial temperature
		int total_iterations = 0; // cummulative number of iteartions
		Bool converged = false;
		while ((temp >= 0)&& (total_iterations < _iterations)&& (!converged)){
			// try different annealing schedules
			total_iterations+=iter;
			converged= am.anneal(iter, temp, this);
			if (!converged) {
			temp = _annealing*temp; // cool off
				iter = _iterations/_updateFreq; // search more elaborately
			} 
		}
		x = am.pb;
	  }
	  //   update the matrices and the number of iterations taken
	  generateMatrices(x);
	  updateMatrix1Fld();
	  updateMatrix2Fld();
	  //float finalobjective = cost(x,this);
	  _finalCostFld->setFloatValue(cost(x,this));
	  _numMarkersFld->setIntValue(_filteredMarkerList->getSize()); //field capturing how many markers were used for cost calculation
	  _finalCostFld->notifyAttachments();
	  _numMarkersFld->notifyAttachments();
	  

  } // end start action

  


  // Handle changes of module parameters and connectors here.
  // For example:
  // if (getInField(0) == field){ /* Handle changes of input 0. */ }
  // if (fieldPtr == field){      /* Handle changes of a field added in constructor .*/ }
  // See section "Implementing handleNotification()" in ML Programming Guide for details.
	BaseOp::handleNotification(field);
}

void ChamferMatching::getValuesFromUI()
{

  _affine1TempM =  _matrix1Fld->getMatrixValue();
  _perspectTempM = _matrix2Fld->getMatrixValue();
  _affine2TempM =  _matrix3Fld->getMatrixValue();
  _matrixToUse = _perspectTempM * _affine1TempM;

  _updateFreq = _updateFreqFld->getIntValue();
  _iterations = _iterationsFld->getIntValue();
  _accuracy = _accuracyFld->getFloatValue();
  _temperature = _temperatureFld->getFloatValue();
  _annealing = _annealingFld->getFloatValue();

  if (_apply1Fld->getBoolValue() == true){
  // Number of independent transformation variables for projection adjustment only. 
	_elementNum = _NUM_AFFINE_PARAMETERS; 

  } 
  if (_apply2Fld->getBoolValue() == true){
  // Number of independent transformation variables for rigid body adjustment only. 
	_elementNum = _NUM_PROJECTION_PARAMETERS;
  } 
  if ((_apply1Fld->getBoolValue() == true)&&(_apply2Fld->getBoolValue() == true)) {
	_elementNum = _NUM_AFFINE_PARAMETERS+ _NUM_PROJECTION_PARAMETERS;
  }
}

//----------------------------------------------------------------------------------
//! Update internal module state after load or clone and enable notification handling again.
//----------------------------------------------------------------------------------
void ChamferMatching::activateAttachments ()
{
  ML_TRACE_IN("ChamferMatching::activateAttachments ()");

  // Update members to new field state here.
  // Call super class functionality to enable notification handling again.
  BaseOp::activateAttachments ();
}

//----------------------------------------------------------------------------------
//! Configures (in)validation handling of inputs which are not connected or up to date.
//----------------------------------------------------------------------------------
BaseOp::INPUT_HANDLE ChamferMatching::handleInput(int /*inIndex*/, INPUT_STATE /*state*/) const
{
  ML_TRACE_IN("ChamferMatching::handleInput ()")

  return INVALIDATE; 
}



//----------------------------------------------------------------------------------
//! define a vector x to modifiy the inital 4*4 tranformation matrices.
//----------------------------------------------------------------------------------
void ChamferMatching::generateMatrices(NRvector<Doub> x)
// This method generates pertutbations to the input affine and perspective transforms
{
	if (x.size() == _NUM_AFFINE_PARAMETERS){
// Generate affine transformation

// Generate perturbations in translations
	mat4 Translations(1,0,0,x[0], 0,1,0,x[1], 0,0,1,x[2], 0, 0, 0,1);
	//_affine1TempM = (_matrix1Fld->getMatrixValue()) * Translations;

// Generate 3 rotations corresponding to x[3], x[4] and x[5]
	// x[3] is the rotation angle in radians around the x-axis
	// x[4] is the rotation angle in radians around the y-axis
	// x[5] is the rotation angle in radians around the z-axis

	// First scale the rotation variables so that they resut in shifts 
	// comprable to the translations 
	
	Vector ImgExt=getInImg(0)->getImgExt();
	MLfloat xscale = ImgExt.x/2.;
	MLfloat yscale = ImgExt.y/2.;
	MLfloat zscale = ImgExt.z/2.;
	getInImg(0)->transformToWorldCoord(&xscale, &yscale, &zscale);
	MLfloat scale = 4.*_DEL /(fabs (xscale) + fabs (yscale));
	// Initiate rotations around the x, y, z, axis
	vec3 axis(1, 0, 0); // x axis
	Rotation RotTemp(axis, x[3] * scale);
	axis.assign(0,1,0); // y axis
	Rotation rot(axis, x[4] * scale); //y axis
	rot *= RotTemp;
	axis.assign(0,0,1); // z axis
	RotTemp.setRotation(axis, x[5]*scale);
	rot *= RotTemp;
	mat4 RotMatrix = rot.getMatrix();
	//_affine1TempM=RotMatrix * _affine1TempM;
	_affine1TempM= (_matrix1Fld->getMatrixValue())*RotMatrix *Translations;
	}
	
	if (x.size() == _NUM_AFFINE_PARAMETERS + _NUM_PROJECTION_PARAMETERS){
		// x[6] is a ux shift in the detector plane
		// x[7] is a vx shift in the detector plane  (piercing point)
		_perspectTempM = _matrix2Fld->getMatrixValue();
		_perspectTempM[0][2]=_perspectTempM[0][2]+x[6]*_perspectTempM[2][2];
		_perspectTempM[1][2]=_perspectTempM[1][2]+x[7]*_perspectTempM[2][2];

	} else if (x.size() == _NUM_PROJECTION_PARAMETERS) {
		_perspectTempM = _matrix2Fld->getMatrixValue();
		_perspectTempM[0][2]=_perspectTempM[0][2]+x[0]*_perspectTempM[2][2];
		_perspectTempM[1][2]=_perspectTempM[1][2]+x[1]*_perspectTempM[2][2];

	}

}



//----------------------------------------------------------------------------------
//! doTransform
//----------------------------------------------------------------------------------
void   ChamferMatching::doTransform()
{
    ML_TRACE_IN("ChamferMatching::doTransform")
	
	_matrixToUse = _perspectTempM * _affine1TempM;
			
	if (_outMarkerList == NULL || _outMarkerList->size() == 0) 
		return;
    
  	for (size_t i = 0; i < _outMarkerList->getSize(); i ++) {
		XMarker *marker3D = (XMarker*)_filteredMarkerList->getItemAt(i);
		XMarker *marker = (XMarker*)_outMarkerList->getItemAt(i);
		Tvec3<double> markerPos(marker3D->x(), marker3D->y(), marker3D->z());
		markerPos = (Tvec3<double>(_matrixToUse * markerPos.affinePoint(), true));
		if (markerPos[2]) {
			markerPos = markerPos.divideByLastComp();
			markerPos[2]=0; //set up the z-coordinate to zero. The actual z coordinate is to be picked up
		}
		// from the original image via the _affine2Temp matrix
		markerPos = (Tvec3<double>(_affine2TempM * markerPos.affinePoint(), true));
		marker->x() = markerPos[0];
		marker->y() = markerPos[1];
		marker->z() = markerPos[2];
		_outMarkerList->modifyItemAt(i, marker);
	}

}


//----------------------------------------------------------------------------------
//! update Matrices
//----------------------------------------------------------------------------------
void ChamferMatching::updateMatrix1Fld() {
	 ML_TRACE_IN("ChamferMatching::updateMatrix1Fld")

	 _matrix1Fld->setMatrixValue(_affine1TempM);
	 _matrix1Fld->notifyAttachments();
}

void ChamferMatching::updateMatrix2Fld() {
	 ML_TRACE_IN("ChamferMatching::updateMatrix2Fld")

	 _matrix2Fld->setMatrixValue(_perspectTempM);
	 _matrix2Fld->notifyAttachments();
}

void ChamferMatching::updateMatrix3Fld() {
	 ML_TRACE_IN("ChamferMatching::updateMatrix3Fld")

	 _matrix3Fld->setMatrixValue(_affine2TempM);
	 _matrix3Fld->notifyAttachments();
}


void ChamferMatching::filter()
{
	_filteredMarkerList = new XMarkerList();

	for (size_t i = 0; i < _inMarkerList->getSize(); i ++) {
		XMarker *tmarker = (XMarker*)_inMarkerList->getItemAt(i);
		Tvec3<double> markerPos(tmarker->x(), tmarker->y(), tmarker->z());
		XMarker *marker3D=new XMarker(markerPos);
		XMarker *marker=new XMarker(markerPos);
		if (!_hasMaskImage) {
			_filteredMarkerList->appendItem(*marker3D);
			} 
		else {
			markerPos = (Tvec3<double>(_matrixToUse * markerPos.affinePoint(), true));
			if(markerPos[2]) {
				markerPos = markerPos.divideByLastComp();
				markerPos[2]=0; //set up the z-coordinate to zero. The actual z coordinate is to be picked up
				// from the original image via the _affine2Temp matrix
			}
			markerPos = (Tvec3<double>(_affine2TempM * markerPos.affinePoint(), true));
			marker->x() = markerPos[0];
			marker->y() = markerPos[1];
			marker->z() = markerPos[2];
			double *voxelValue = NULL;
		
			if (getVoxelValue(1,marker, _maskImg, &voxelValue)) {
				if (*voxelValue==0) {
					// we need to keep this marker from original list
					_filteredMarkerList->appendItem(*marker3D);
				}
			} 
			delete marker3D;
			delete marker;
			
		}
	}
	_outMarkerList = new XMarkerList();
	for (size_t i = 0; i < _filteredMarkerList->getSize(); i ++) {
		XMarker *marker1=new XMarker();
		_outMarkerList->appendItem(*marker1);
		delete marker1;
		}
}
ML_END_NAMESPACE

