/**********************************************************
 * Version $Id$
 *********************************************************/

///////////////////////////////////////////////////////////
//                                                       //
//                         SAGA                          //
//                                                       //
//      System for Automated Geoscientific Analyses      //
//                                                       //
//                     Tool Library                      //
//                     grid analysis                     //
//                                                       //
//-------------------------------------------------------//
//                                                       //
//                    Grid_IMCORR.cpp                    //
//                                                       //
//                 Copyright (C) 2012 by                 //
//                      Magnus Bremer                    //
//                                                       //
//-------------------------------------------------------//
//                                                       //
// This file is part of 'SAGA - System for Automated     //
// Geoscientific Analyses'. SAGA is free software; you   //
// can redistribute it and/or modify it under the terms  //
// of the GNU General Public License as published by the //
// Free Software Foundation, either version 2 of the     //
// License, or (at your option) any later version.       //
//                                                       //
// SAGA is distributed in the hope that it will be       //
// useful, but WITHOUT ANY WARRANTY; without even the    //
// implied warranty of MERCHANTABILITY or FITNESS FOR A  //
// PARTICULAR PURPOSE. See the GNU General Public        //
// License for more details.                             //
//                                                       //
// You should have received a copy of the GNU General    //
// Public License along with this program; if not, see   //
// <http://www.gnu.org/licenses/>.                       //
//                                                       //
//-------------------------------------------------------//
//                                                       //
//    e-mail:     magnus.bremer@uibk.ac.at               //
//                                                       //
//    contact:    Magnus Bremer                          //
//                Institute of Geography                 //
//                University of Innsbruck                //
//                Innrain 52                             //
//                6020 Innsbruck                         //
//                Austria                                //
//                                                       //
///////////////////////////////////////////////////////////

//---------------------------------------------------------
/*	This implementation is based on the original FORTRAN code
available from <http://nsidc.org/data/velmap/imcorr.html>.
This implementation extends the functionality by introducing
the possibility to derive 3D displacement vectors once DTM
datasets are provided.
*/
//---------------------------------------------------------


///////////////////////////////////////////////////////////
//														 //
//														 //
//														 //
///////////////////////////////////////////////////////////

//---------------------------------------------------------
#include "Grid_IMCORR.h"


///////////////////////////////////////////////////////////
//														 //
//														 //
//														 //
///////////////////////////////////////////////////////////

//---------------------------------------------------------
CGrid_IMCORR::CGrid_IMCORR(void)
{
	Set_Name		(_TL("IMCORR - Feature Tracking"));

	Set_Author		("M. Bremer, V. Wichmann (c) 2012-2026");

	Set_Version		(SG_T("1.1"));

	Set_Description	(_TW(
		"The tool performs displacement analysis using image cross-correlation between two raster datasets. "
		"For each raster cell, a reference window and a search window are defined. The reference window "
		"extracts a template pattern, which is then matched within the larger search window. "
		"The algorithm identifies the correlation peak and uses its position to calculate the magnitude "
		"and direction of displacement.\n"
		"The maximum displacement of a feature that can be measured is:\n"
		"(search chip size/2 - reference chip size/2) * cell size\n"
		"This maximum applies only to exactly diagonal motion. In order to measure larger distances, a known "
		"offset between reference and search window can be given. The offset specifies the x/y components of the "
		"expected movement. Besides a constant offset, spatially variable offsets can be applied when a "
		"point shapes layer with start positions and x- and y-displacements, e.g. from a previous tool run, "
		"is provided.\n"
		"The tool can take two elevation models to query the elevations at the start and end points of the "
		"each detected movement to compute a 3D displacement vector.\n"
		"The tool has been ported to SAGA from the standalone IMCORR software developed by the National Snow "
		"and Ice Data Center in Boulder, Colorado, United States, and extended to take also elevation and "
		"spatially variable offsets into account.\n\n"
		"The tool computes the following attributes (some of them are only available if elevation data is provided):\n"
		"<ul>"
		"<li>ID: a unique identifier per measurement</li>"
		"<li>GX, GY: grid column and row indices of the source (start) cell</li>"
		"<li>REALX, REALY, REALZ: x,y,z world coordinates of the source (start) cell of the movement</li>"
		"<li>DISP: horizontal displacement distance [map units]</li>"
		"<li>DISP_REAL: 3D displacement distance [map units]</li>"
		"<li>STRENGTH: correlation strength of the Fourier analysis [-]</li>"
		"<li>FLAG: flag indicating a correlation was found [-]</li>"
		"<li>XDISP, YDISP, ZDISP: x,y,z components of the displacement [map units]</li>"
		"<li>XDISP_UNIT, YDISP_UNIT: x,y components of the displacement unit vector [0-1]</li>"
		"<li>XTARG, YTARG, ZTARG: x,y,z world coordinates of the target (stop) cell of the movement</li>"
		"<li>XERR, YERR: x,y error components of the correlation strength [-]</li>"
		"<li>ASPECT, SLOPE: compass direction and gradient of the displacement [degree]</li>"
		"<li>ID_M: identifier of the spatially variable measurement position (if specified)</li>"
		"</ul>\n\n"
		"Note: the tool requires that the input data does not contain any NoData cells."
	));

	Add_Reference(
		"Scambos, T. A., Dutkiewicz, M. J., Wilson, J. C., and Bindschadler, R. A.", "1992",
		"Application of image cross-correlation to the measurement of glacier velocity using satellite image data",
		"Remote Sensing Environ., 42(3), 177-186.",
		SG_T("https://doi.org/10.1016/0034-4257(92)90101-O"), SG_T("Link")
	);

	Add_Reference(
		"Fahnestock, M. A., Scambos, T. A., and Bindschadler, R. A.", "1992",
		"Semi-automated ice velocity determination from satellite imagery",
		"Eos, 73, 493."
	);

	Add_Reference(
		"Fey, C., Rutzinger, M., Wichmann, V., Prager, C., Bremer, M., and Zangerl, C.", "2015",
		"Deriving 3D displacement vectors from multi-temporal airborne laser scanning data for landslide activity analyses",
		"GIScience & Remote Sensing, 52(4), 437-461.",
		SG_T("https://doi.org/10.1080/15481603.2015.1045278"), SG_T("Link")
	);

	Parameters.Add_Grid("",
		"GRID_1", _TL("Grid 1"),
		_TL("The first grid to correlate."),
		PARAMETER_INPUT
	);

	Parameters.Add_Grid("",
		"GRID_2"	, _TL("Grid 2"),
		_TL("The second grid to correlate."),
		PARAMETER_INPUT
	);

	Parameters.Add_Grid("",
		"DTM_1", _TL("DTM 1"),
		_TL("The DEM used to extract elevation information for grid 1."),
		PARAMETER_INPUT_OPTIONAL
	);

	Parameters.Add_Grid("",
		"DTM_2"	, _TL("DTM 2"),
		_TL("The DEM used to extract elevation information for grid 2."),
		PARAMETER_INPUT_OPTIONAL
	);

	Parameters.Add_Shapes("",
		"CORRPOINTS", _TL("Correlated Points"),
		_TL("Point shapes layer of correlated positions with displacement and correlation information."),
		PARAMETER_OUTPUT, SHAPE_TYPE_Point
	);

	Parameters.Add_Shapes("",
		"CORRLINES"	, _TL("Displacement Vector"),
		_TL("Line shapes layer with displacement vectors between correlated points."),
		PARAMETER_OUTPUT, SHAPE_TYPE_Line
	);

	Parameters.Add_Choice("",
		"SEARCH_CHIPSIZE", _TL("Search Chip Size"),
		_TL("The size of the search chip (rectangular search area) used to find the correlating reference chip (pattern template) [cells]."),
		CSG_String::Format("%s|%s|%s|%s",
			SG_T("32x32"),
			SG_T("64x64"),
			SG_T("128x128"),
			SG_T("256x256")
		), 1
	);

	Parameters.Add_Choice("",
		"REF_CHIPSIZE"	, _TL("Reference Chip Size"),
		_TL("The size of the reference chip, i.e. the pattern template to find in search chip [cells]."),
		CSG_String::Format("%s|%s|%s|%s",
			SG_T("16x16"),
			SG_T("32x32"),
			SG_T("64x64"),
			SG_T("128x128")
		), 1	
	);

	Parameters.Add_Int("",
		"STEPPING"	, _TL("Stepping"),
		_TL("The grid cell stepping used to move the search chip over the input grids [cells]."),
		2, 1, true
	);

	Parameters.Add_Choice("",
		"OFFSET_TYPE", _TL("Offset Type"),
		_TL("Determines whether no offset between reference and search chip is applied, a uniform offset is used, or the offset varies across the spatial extent."),
		CSG_String::Format("%s|%s|%s",
			SG_T("no offset"),
			SG_T("constant offset"),
			SG_T("spatially variable offset")
		), 0
	);

	Parameters.Add_Double("OFFSET_TYPE",
		"DISP_X",	_TL("X Displacement"),
		_TL("The expected displacement in the x-direction (positive = east) [map units]."),
		0.0
	);

	Parameters.Add_Double("OFFSET_TYPE",
		"DISP_Y",	_TL("Y Displacement"),
		_TL("The expected displacement in the y-direction (positive = north) [map units]."),
		0.0
	);

	Parameters.Add_Shapes("OFFSET_TYPE",
		"SHP_OFFSET",	_TL("Displacement Offsets"),
		_TL("A point shapes layer with start positions and pre-calculated displacement offsets."),
		PARAMETER_INPUT, SHAPE_TYPE_Point
	);

	Parameters.Add_Table_Field("SHP_OFFSET",
		"FIELD_DISP_X",	_TL("X Displacement"),
		_TL("The attribute specifying the expected displacement in the x-direction (positive = east) [map units].")
	);

	Parameters.Add_Table_Field("SHP_OFFSET",
		"FIELD_DISP_Y",	_TL("Y Displacement"),
		_TL("The attribute specifying the expected displacement in the y-direction (positive = north) [map units].")
	);

	Parameters.Add_Table_Field("SHP_OFFSET",
		"FIELD_ID",	_TL("ID"),
		_TL("The identifier of the measurement position."),
		true
	);

	Parameters.Add_Double("OFFSET_TYPE",
		"MAX_DIST",	_TL("Maximum Distance"),
		_TL("The maximum distance of a raster cell node to a pre-calculated start position [map units]."),
		4.0, 0.0, true
	);
}

///////////////////////////////////////////////////////////
//														 //
///////////////////////////////////////////////////////////

//---------------------------------------------------------
int CGrid_IMCORR::On_Parameters_Enable(CSG_Parameters *pParameters, CSG_Parameter *pParameter)
{
	if(	pParameter->Cmp_Identifier("OFFSET_TYPE") )
	{
		pParameters->Set_Enabled("DISP_X"		, pParameter->asInt() == 1);
		pParameters->Set_Enabled("DISP_Y"		, pParameter->asInt() == 1);
		pParameters->Set_Enabled("SHP_OFFSET"	, pParameter->asInt() == 2);
		pParameters->Set_Enabled("MAX_DIST"		, pParameter->asInt() == 2);
	}

	return( CSG_Tool_Grid::On_Parameters_Enable(pParameters, pParameter) );
}


///////////////////////////////////////////////////////////
//														 //
///////////////////////////////////////////////////////////

//---------------------------------------------------------
bool CGrid_IMCORR::On_Execute(void)
{
	const double mincorr = 2.0;
	const int	 fitmeth = 1;
	const double maxdis  = -1.0;

	CSG_Grid	*pGrid1			= Parameters("GRID_1")->asGrid();
	CSG_Grid	*pGrid2			= Parameters("GRID_2")->asGrid();
	CSG_Grid	*pDTM1			= Parameters("DTM_1")->asGrid();
	CSG_Grid	*pDTM2			= Parameters("DTM_2")->asGrid();
	CSG_Shapes	*pCorrPts		= Parameters("CORRPOINTS")->asShapes();
	CSG_Shapes	*pCorrLines		= Parameters("CORRLINES")->asShapes();
	int			Search_Chipsize	= Parameters("SEARCH_CHIPSIZE")->asInt() + 1;
	int			Ref_Chipsize	= Parameters("REF_CHIPSIZE")->asInt();
	int			Stepping		= Parameters("STEPPING")->asInt();
	int			iTypeOffset		= Parameters("OFFSET_TYPE")->asInt();
	int			xOffset			= 0;
	int			yOffset			= 0;
	CSG_Shapes	*pOffsetPts		= Parameters("SHP_OFFSET")->asShapes();
	int			iFieldDispX		= Parameters("FIELD_DISP_X")->asInt();
	int			iFieldDispY		= Parameters("FIELD_DISP_Y")->asInt();
	int			iFieldID		= Parameters("FIELD_ID")->asInt();
	double		dMaxDistCorrPt	= Parameters("MAX_DIST")->asDouble();

	if (iTypeOffset == 1)
	{
		xOffset = std::lround(Parameters("DISP_X")->asDouble() / Get_Cellsize());
		yOffset = std::lround(Parameters("DISP_Y")->asDouble() / Get_Cellsize());
	}

	Search_Chipsize  = (int)pow(2.0, 4 + Search_Chipsize);
	Ref_Chipsize	 = (int)pow(2.0, 4 + Ref_Chipsize);


	//---------------------------------------------------------
	if (Search_Chipsize < Ref_Chipsize)
	{
		SG_UI_Msg_Add_Error(_TL("The search chip must be larger than the reference chip."));
		return (false);

		// note: correlation fails when search and reference chip have the same size but show
		// a displacement because in the code we are essentially asking "are the two patches 
		// identical without shifting?" when testing shifts / alignment positions
	}

	if ( (pDTM1 != NULL && pDTM2 == NULL) || (pDTM1 == NULL && pDTM2 != NULL) )
	{
		SG_UI_Msg_Add_Error(_TL("Only one elevation model provided, either provide both or none."));
		return (false);
	}

	//---------------------------------------------------------
	TSG_Vertex_Type VertexType = pDTM1 ? SG_VERTEX_TYPE_XYZ : SG_VERTEX_TYPE_XY;

	pCorrPts->Create(SHAPE_TYPE_Point, CSG_String::Format("%s_CORRPOINTS", pGrid1->Get_Name()).c_str(), (CSG_Table *)0, VertexType);
	_Add_Fields(pCorrPts, VertexType > 0, iTypeOffset == 2 && iFieldID > -1);

	pCorrLines->Create(SHAPE_TYPE_Line, CSG_String::Format("%s_DISP_VEC", pGrid1->Get_Name()).c_str(), (CSG_Table *)0, VertexType);
	_Add_Fields(pCorrLines, VertexType > 0, iTypeOffset == 2 && iFieldID > -1);
	
	
	//---------------------------------------------------------
	int lMargin = Search_Chipsize / 2 - (double)xOffset < Ref_Chipsize / 2 ? Ref_Chipsize / 2 : (int)(Search_Chipsize / 2 - (double)xOffset);
	int rMargin = Search_Chipsize / 2 + (double)xOffset < Ref_Chipsize / 2 ? Ref_Chipsize / 2 : (int)(Search_Chipsize / 2 + (double)xOffset);
	int bMargin = Search_Chipsize / 2 - (double)yOffset < Ref_Chipsize / 2 ? Ref_Chipsize / 2 : (int)(Search_Chipsize / 2 - (double)yOffset);
	int tMargin = Search_Chipsize / 2 + (double)yOffset < Ref_Chipsize / 2 ? Ref_Chipsize / 2 : (int)(Search_Chipsize / 2 + (double)yOffset);

	//---------------------------------------------------------
	CSG_KDTree_2D Search;

	if (iTypeOffset == 2)
	{
		Search.Create(pOffsetPts);
	}
	
	//---------------------------------------------------------
	std::vector<RESULT> vResults(pGrid1->Get_NCells());

	for(int x=lMargin-1; x<pGrid1->Get_NX()-rMargin && Set_Progress(x, pGrid1->Get_NX()-rMargin); x+=Stepping)
	{
		#ifndef _DEBUG
		#pragma omp parallel for
		#endif
		for(int y=bMargin-1; y<pGrid1->Get_NY()-tMargin; y+=Stepping)
		{
			int		xoff	= xOffset;
			int		yoff	= yOffset;
			sLong	id_m	= 0;
			
			if (iTypeOffset == 2)
			{
				size_t Index; double Distance;

				if (!Search.Get_Nearest_Point(Get_System().Get_xGrid_to_World(x), Get_System().Get_yGrid_to_World(y), Index, Distance))
				{
					continue;
				}

				if (Distance > dMaxDistCorrPt)
				{
					continue;
				}

				CSG_Shape *pPt = pOffsetPts->Get_Shape((sLong)Index);

				xoff = std::lround(pPt->asDouble(iFieldDispX) / Get_Cellsize());
				yoff = std::lround(pPt->asDouble(iFieldDispY) / Get_Cellsize());
				
				if (x + xoff - Search_Chipsize / 2 < 0 || x + xoff + Search_Chipsize / 2 > Get_NX() - 1) { continue; }
				if (y + yoff - Search_Chipsize / 2 < 0 || y + yoff + Search_Chipsize / 2 > Get_NY() - 1) { continue; }

				if (iFieldID > -1)
				{
					id_m = pPt->asLong(iFieldID);
				}
			}


			int	   success  = 1;
			double ddact    = 0.0;
			double strength = 0.0;

			std::vector<double> best_fit, est_err;
			std::vector<double> nomoff {0.0, (Search_Chipsize-Ref_Chipsize) * 0.5, (Search_Chipsize-Ref_Chipsize) * 0.5};
			std::vector<double> ioffrq {0.0,  Search_Chipsize               * 0.5,  Search_Chipsize               * 0.5};

			std::vector<double> ChipSearch, ChipRef;

			Get_Chip(ChipRef   , pGrid1, x       , y       , Ref_Chipsize   );
			Get_Chip(ChipSearch, pGrid2, x + xoff, y + yoff, Search_Chipsize);
			
			gcorr(ChipSearch, ChipRef, mincorr, fitmeth, maxdis, ioffrq, nomoff, success, strength, best_fit, est_err, ddact);
			
			sLong n = Get_System().Get_IndexFromRowCol(x, y);

			if (success == 1)
			{
				best_fit[1] += yoff;
				best_fit[2] += xoff;

				RESULT *pRes = &vResults[n];

				pRes->success  = true;
				pRes->x        = x;
				pRes->y        = y;
				pRes->id_m     = id_m;
				pRes->xDisp    = best_fit[2] * Get_Cellsize();
				pRes->yDisp    = best_fit[1] * Get_Cellsize();
				pRes->strength = strength;
				pRes->xerr     = est_err[2];
				pRes->yerr     = est_err[1];
				pRes->disp     = sqrt(best_fit[1]*best_fit[1] + best_fit[2]*best_fit[2]) * Get_Cellsize();
				pRes->DirNormX = (best_fit[2] * Get_Cellsize()) / pRes->disp;
				pRes->DirNormY = (best_fit[1] * Get_Cellsize()) / pRes->disp;
				pRes->xReal    = pGrid1->Get_System().Get_xGrid_to_World(x);
				pRes->yReal    = pGrid1->Get_System().Get_yGrid_to_World(y);
				pRes->xReal2   = pRes->xReal + best_fit[2] * Get_Cellsize();
				pRes->yReal2   = pRes->yReal + best_fit[1] * Get_Cellsize();
				
				if((pRes->DirNormY > 0.0 && pRes->DirNormX > 0))
				{
					pRes->Aspect = (atan(fabs(pRes->DirNormX) / fabs(pRes->DirNormY))) * M_RAD_TO_DEG;
				}
				else if ((pRes->DirNormY <= 0.0 && pRes->DirNormX > 0))
				{
					pRes->Aspect = 90 + (atan(fabs(pRes->DirNormY) / fabs(pRes->DirNormX))) * M_RAD_TO_DEG;
				}
				else if ((pRes->DirNormY <= 0.0 && pRes->DirNormX <= 0))
				{
					pRes->Aspect = 180 + (atan(fabs(pRes->DirNormX) / fabs(pRes->DirNormY))) * M_RAD_TO_DEG;
				}
				else
				{
					pRes->Aspect = 270 + (atan(fabs(pRes->DirNormY) / fabs(pRes->DirNormX))) * M_RAD_TO_DEG;
				}

				if (pDTM1)
				{
					pRes->zReal    = pDTM1->asDouble(x, y);
					pRes->zReal2   = pDTM2->Get_Value(pRes->xReal2, pRes->yReal2);
					pRes->dispReal = sqrt(pow(pRes->zReal2 - pRes->zReal, 2) + pRes->disp * pRes->disp);
					pRes->Slope    = atan((pRes->zReal2 - pRes->zReal) / fabs(pRes->disp)) * M_RAD_TO_DEG;
				}
			}
			else
			{
				vResults[n].success = false;
			}
		}
	}

	//---------------------------------------------------------
	sLong iShape = 0;

	for(size_t i=0; i<vResults.size(); i++)
	{
		if( vResults[i].success )
		{
			_Add_Shape(pCorrPts  , SHAPE_TYPE_Point, VertexType > 0, iTypeOffset == 2 && iFieldID > -1, iShape, vResults[i]);
			_Add_Shape(pCorrLines, SHAPE_TYPE_Line , VertexType > 0, iTypeOffset == 2 && iFieldID > -1, iShape, vResults[i]);
			iShape++;
		}
	}

	return( true );
}

//---------------------------------------------------------
void CGrid_IMCORR::gcorr(std::vector<double> ser, std::vector<double> ref, double csmin, int mfit, double ddmx,
	                     std::vector<double> ioffrq, std::vector<double> nomoff, int& iacrej, double& streng, std::vector<double>& bfoffs,
	                     std::vector<double>& tlerrs, double ddact)
{
	bfoffs.resize(3);

	// compute raw cross-product sums
	std::vector<double> unormc;

	cross(unormc, ser, ref);

	// compute normalized cross-correlation values and compile statistics
	std::vector<double> ccnorm;
	std::vector<double> pkval;
	std::vector<int>    ipkcol;
	std::vector<int>    ipkrow;
	std::vector<double> sums;

	gnorm(ccnorm, pkval, ipkcol, ipkrow, sums, ser, ref, unormc);

	int ncol = (int)sqrt(ser.size() - 1) - (int)sqrt(ref.size() - 1) + 1;
	int nrow = ncol;

	// evaluate strength of correlation peak
	std::vector<double> cpval;

	eval(ncol, nrow, ccnorm, pkval, ipkcol, ipkrow, sums, csmin, streng, iacrej, cpval);

	// determine offsets of peak relative to nominal location
	if (iacrej==1) 
	{
		if (mfit != 4)
		{
			std::vector<double> pkoffs;

			fitreg(cpval, mfit, pkoffs, tlerrs);
			
			bfoffs[1] = (ipkcol[1] - 1) - nomoff[1] + pkoffs[1];
			bfoffs[2] = (ipkrow[1] - 1) - nomoff[2] + pkoffs[2];
		}
		else
		{
			bfoffs[1] = (ipkcol[1] - 1) - nomoff[1];
			bfoffs[2] = (ipkrow[1] - 1) - nomoff[2];
			tlerrs[1] = 0.5;
			tlerrs[2] = 0.5;
		}
		
		// determine diagonal displacement from nominal and check against maximum acceptable value
		ddact = sqrt(bfoffs[1]*bfoffs[1] + bfoffs[2]*bfoffs[2]);
		
		if (ddmx > 0.0)
		{
	  		if (ddact > ddmx)
			{
	      		iacrej = 5;
			}
		}
		else
		{
	  		if ( (bfoffs[1]*bfoffs[1] > ioffrq[1]*ioffrq[1])|| (bfoffs[2]*bfoffs[2] >  ioffrq[2]*ioffrq[2]) )
			{
	      		iacrej = 5;
			}
		}
	}

	return;
}

//---------------------------------------------------------
void CGrid_IMCORR::fitreg(std::vector<double> cpval, int mfit, std::vector<double>& pkoffs, std::vector<double>& tlerrs)
{
	pkoffs.resize(3);
	tlerrs.resize(3);
	
	int i, j;
	std::vector<std::vector<float> > b;
	std::vector<double> coeffs, vector, wghts, z;
	coeffs.resize(7);
	double denom;

	// compute elements of matrix and column vector;
	sums(cpval, mfit, z, wghts, b, vector);

	// invert matrix to get best-fit peak offsets
	kvert(b);
	i = 1;

	while (i <= 6)
	{
		coeffs[i] = 0.0;
		j = 1;
		while(j <= 6)
		{
			coeffs[i] += (b[i-1][j-1]*vector[j]);
			j++;
		}
		i++;
	}

	denom     = 4.0 * coeffs[4] * coeffs[6] - coeffs[5] * coeffs[5];
	pkoffs[1] = (coeffs[3] * coeffs[5] - 2.0 * coeffs[2] * coeffs[6]) / denom;
	pkoffs[2] = (coeffs[2] * coeffs[5] - 2.0 * coeffs[3] * coeffs[4]) / denom;

	// estimate errors in best fit offsets
	esterr(z, wghts, b, coeffs, pkoffs, tlerrs);

	return;
}

//---------------------------------------------------------
void CGrid_IMCORR::esterr(std::vector<double> z, std::vector<double> wghts,std::vector<std::vector<float> > bnvrs, std::vector<double> coeffs, std::vector<double>& pkoffs, std::vector<double>& tlerrs)
{
	pkoffs.resize(3);
	tlerrs.resize(4);

	int i, ivalpt, j;
	std::vector<double> du, dv;
	du.resize(7);
	dv.resize(7);
	double c, denom, f, usum, var, vsum, x, xsum, y;

	// compute residual variance for elements of peak array
	ivalpt = 1;
	var    = 0.0;
	y      = -2.0;

	while (y <= 2.0)
	{
		x = -2.0;

		while(x <= 2.0)
		{
			f      =  coeffs[1] + coeffs[2]*x + coeffs[3]*y+ coeffs[4]*x+x + coeffs[5]*x*y + coeffs[6]*y*y;
			var    += (wghts[ivalpt]*pow((f-z[ivalpt]),2));
			ivalpt += 1;
			x      += 1.0;
		}

		y += 1.0;
	}

	// compute constants to use the weights
	c = var/19.0;

	// compute partial derivatives of peak offsets with respect to polynomial coefficients
	denom = 4.0 * coeffs[4] * coeffs[6] - (coeffs[5] * coeffs[5]);
	du[1] = 0.0;
	du[2] = -2.0 * coeffs[6] / denom;
	du[3] = coeffs[5] / denom;
	du[4] = -4.0 * coeffs[6] * pkoffs[1] / denom;
	du[5] = (coeffs[3] + 2.0 * coeffs[5] * pkoffs[1]) / denom;
	du[6] = (-2.0 * coeffs[2] -4.0 * coeffs[4] * pkoffs[1]) / denom;

	dv[1] = 0.0;
	dv[2] = du[3];
	dv[3] = -2.0 * coeffs[4]/denom;
	dv[4] = (-2.0 * coeffs[3] - 4.0 * coeffs[6] * pkoffs[2]) / denom;
	dv[5] = (coeffs[2] + 2.0 * coeffs[5] * pkoffs[2]) / denom;
	dv[6] = -4.0 * coeffs[4] * pkoffs[2] / denom;

	// compute estimated errors in offsets
	usum = 0.0;
	vsum = 0.0;
	xsum = 0.0;
	i=1;

	while (i <= 6)
	{
		j=1;

		while(j <= 6)
		{
			usum += (du[i] * du[j] * bnvrs[i-1][j-1]);
			vsum += (dv[i] * dv[j] * bnvrs[i-1][j-1]);
			xsum += (du[i] * dv[j] * bnvrs[i-1][j-1]);
			j++;
		}

		i++;
	}
	tlerrs[1] = sqrt(abs(c*usum));
	tlerrs[2] = sqrt(abs(c*vsum));
	tlerrs[3] = c*xsum;

	return;
}


//---------------------------------------------------------
void CGrid_IMCORR::sums(std::vector<double> cpval, int mfit, std::vector<double>& z, std::vector<double>& wghts,std::vector<std::vector<float> > & b,
	                    std::vector<double>& vector)
{
	b.resize(6);

	for(size_t i=0; i<b.size(); i++)
	{
		b[i].resize(6);
	}

	vector.resize(26);
	wghts.resize(26);
	z.resize(26);

	int i, ic, ir, ivalpt, j;

	std::vector<double> term;
	term.resize(7);

	double val;

	//initialize arrays and constants
	i = 1;

	while (i <= 6)
	{
		j=1;

		while(j <= 6)
		{
			b[i-1][j-1] = 0.0;
			j++;
		}
		vector[i] = 0.0;
		i++;
	}
	term[1] = 1.0;
	ivalpt  = 0;

	//compute function of correlation coefficient and assign weight for each location in neighbourhood of peak
	ir = 1;

	while (ir <= 5)
	{
		ic = 1;

		while (ic <= 5)
		{
			ivalpt++;
			
			if (cpval[ivalpt] > 1.0)
			{
				val = cpval[ivalpt];
			}
			else
			{
				val = 1.0;
			}

			if (mfit == 1)      // elliptical paraboloid
			{
				z[ivalpt]     = val;
				wghts[ivalpt] = 1.0;
			}
			else if (mfit == 2) // elliptical gaussian
			{
				z[ivalpt]     = log(val);
				wghts[ivalpt] = pow(val,2);
			}
			else                // reciprocal paraboloid
			{
				z[ivalpt]     = 1.0/val;
				wghts[ivalpt] = pow(val,4);
			}

			// compute matrix and vector elements in linear equations for best-fit coefficients
			term[2] = ic -3;
			term[3] = ir -3;
			term[4] = term[2] * term[2];
			term[5] = term[2] * term[3];
			term[6] = term[3] * term[3];
			i = 1;

			while (i <= 6)
			{	
				vector[i] += (wghts[ivalpt]*term[i]*z[ivalpt]);
				j=1;

				while (j <= 6)
				{	
					b[i-1][j-1] += (float)(wghts[ivalpt]*term[i]*term[j]);
					j++;
				}
				i++;
			}
			ic++;
		}
		ir++;
	}

	return;	
}


//---------------------------------------------------------
void  CGrid_IMCORR::cofact( float num[25][25], float f, std::vector<std::vector<float> >& INV)
{
    float b[25][25], fac[25][25];
    int p, q, m, n, i, j;
 
    for (q=0; q<f; q++)
    {
        for (p=0; p<f; p++)
        {
            m = 0;
            n = 0;
 
            for (i=0; i<f; i++)
            {
                for (j=0; j<f; j++)
                {
                    b[i][j] = 0;
 
                    if (i != q && j != p)
                    {
                        b[m][n] = num[i][j];
 
                        if ( n < ( f-2.0 ) )
                            n++;
                        else
                        {
                            n = 0;
                            m++;
                        }
                    }
                }
            }
 
            fac[q][p] = (float)(pow( -1.0, (q + p) ) * detrm( b, (f-1) ));
        }
    }
 
    trans(num, fac, f, INV);

	return;
}


//---------------------------------------------------------
float  CGrid_IMCORR::detrm(float a[25][25], float k)
{
    float s = 1, det = 0, b[25][25];
    int i, j, m, n, c;
 
    if ( k == 1 )
    {
        return ( a[0][0] );
    }
    else
    {
        det = 0;
 
        for (c=0; c<k; c++)
        {
            m = 0;
            n = 0;
 
            for (i=0; i<k; i++)
            {
                for (j=0; j<k; j++)
                {
                    b[i][j] = 0;
 
                    if ( i != 0 && j != c )
                    {
                        b[m][n] = a[i][j];
 
                        if ( n < ( k-2 ) )
                            n++;
                        else
                        {
                            n = 0;
                            m++;
                        }
                    }
                }
            }
 
            det = det + s * ( a[0][c] * detrm( b, k-1 ) );
            s = -1 * s;
        }
    }
 
    return ( det );
}


//---------------------------------------------------------
void  CGrid_IMCORR::trans(float num[25][25], float fac[25][25], float r, std::vector<std::vector<float> >& INV)
{
    int i, j = 0;
    float b[25][25], inv[25][25], d;
 
    for (i=0; i<r; i++)
    {
        for (j=0; j<r; j++)
        {
            b[i][j] = fac[j][i];
        }
    }
 
    d = detrm(num, r);
    inv[i][j] = 0;
 
    for (i=0; i<r; i++)
    {
        for (j=0; j<r; j++)
        {
            inv[i][j] = b[i][j] / d;
        }
    }
 
	INV.resize((int)r);

	for (int ii=0; ii<r; ii++)
	{
		INV[ii].resize((int)r);
	}
    for (i=0;i<r; i++)
    {
        for (j= 0; j<r; j++)
        {
            INV[i][j] = inv[i][j];
        }
    }

	return;
}


//---------------------------------------------------------
void CGrid_IMCORR::kvert(std::vector<std::vector<float> >& V)
{
	const int k = (int)V[0].size();
	float b[25][25];

	for (int i=0; i<k; i++)
    {
        for (int j=0; j<k; j++)
        {
             b[i][j] = V[i][j];
        }
    }
	
	float d = detrm( b, (float)k );
    
	if ( d != 0 )
	{
        cofact(b, (float)k, V);
	}

	return;
}


//---------------------------------------------------------
// code from original FORTRAN version
//void CGrid_IMCORR::kvert(std::vector<std::vector<double> >& V, std::vector<double>W)
//{
//	int LV = V[0].size()-1;
//	int N = V.size()-1;
//	int H,I,J,K,L,M,N,O,P,Q;
//	double S, T;
//
//
//	if (N != 1)
//	{
//		O = N+1;
//		L = 0;
//		M = 1;
//		if (L != N)
//		{
//			K = L;
//			L = M;
//			M = M + 1;
//			// find pivot and start row swap
//			P = L;
//			Q = L;
//			S = abs(V[L][L]);
//			H = L;
//			bool GoOut = false;
//			while (H <= N)
//			{
//				I = L;
//				while(I <= N)
//				{	
//					T = abs(V[I][H]);
//					if (T <= S)
//						break;
//					P = I;
//					Q = H;
//					S = T;
//					if ( I>N)
//						break;
//					I++;
//				}
//				H++;
//			}
//			W[N+L] = P;
//			W[O-L] = Q;
//			I = 1;
//			while(I <= N)
//			{
//				T = V[I][L];
//				V[I][L] = V[I][Q];
//				V[I][Q] = T;
//				I++;
//			}
//			S = V[P][L];
//			V[P][L] = V[L][L];
//			if (S != 0.0)
//			{
//				// compute multipliers
//				V[L][L] = -1;
//				S = 1.0/S;
//				I=1;
//				while(I <= N)
//				{
//					V[I][L] = -S * V[I][L];
//					I++;
//				}
//				J=L;
//				J++;
//				if(J > N)
//					J = 1;
//
//					
//			}
//
//		}
//	}
//	return;
//}


//---------------------------------------------------------
void CGrid_IMCORR::eval(int ncol, int nrow, std::vector<double> CCNORM, std::vector<double> pkval, std::vector<int> ipkcol,
	                    std::vector<int> ipkrow, std::vector<double> sums, double& csmin, double& streng, int& iacrej, std::vector<double>& cpval)
{
	cpval.resize(26,0.0); // shift to mimic fortran
	std::vector<int> ipt5(3);
	int i, icol, idist, iptr, ipt7, irow, krbase, lcol, lrow, npts, n5, n7, j;
	double bmean, bsigma;
	ipt5[0] = 0;
	ipt5[1] = 32;
	ipt5[2] = 32;
	iacrej  = 1;
	streng  = 0.0;
	ipt7    = 1;

	// check for peak value within two rows or columns of edge
	if ((ipkcol[1] <= 2) || (ipkcol[1] >= ncol-1) || (ipkrow[1] <= 2) || (ipkrow[1] >= nrow-1))
	{
		iacrej = 2;
		return;
	}

	// find largest values outside 5 by 5 and 7 by 7 neighbourhoods of peak
	n5 = 0;
	n7 = 0;
	i=2;
	while ((n5 < 2) && (i <= 32))
	{
		if (abs(ipkcol[1]-ipkcol[i]) > abs(ipkrow[1]-ipkrow[i]))
			idist = abs(ipkcol[1]-ipkcol[i]);
		else
			idist = abs(ipkrow[1]-ipkrow[i]);

		if (idist > 2)
		{
			n5       = n5 + 1;
			ipt5[n5] = i;

			if ((n7 == 0) && (idist < 3))
			{
				ipt7 = i;
				n7   = 1;
			}
		}
		i++;
	}

	if((ipt5[1] <= 3) || ipt5[2] <=5)
	{
		iacrej = 3;
		return;
	}

	// find borders of 9 by 9 array centred on peak
	if (1 > ipkcol[1] - 4)
		icol = 1;
	else
		icol = ipkcol[1] - 4;

	if (1 > ipkcol[1] - 4)
		irow = 1;
	else
		irow = ipkcol[1] - 4;

	if (ncol < ipkcol[1] - 4)
		lcol = ncol;
	else
		lcol = ipkcol[1] - 4;

	if (nrow < ipkcol[1] - 4)
		lrow = nrow;
	else
		lrow = ipkcol[1] - 4;

	// eliminate points within 9 by 9 array from background statistics
	krbase = ncol * (nrow - 1);
	i = irow;

	while (i <= lrow)
	{
		j=icol;

		while (j <= lcol)
		{
			sums[0] -= CCNORM[krbase+j];
			sums[1] -= CCNORM[krbase+j] * CCNORM[krbase+j];
			j++;
		}

		krbase += ncol;
		i++;
	}

	// compute background mean and standard deviation
	npts   = ncol * nrow - (lcol - icol +1) * (lrow - irow + 1);
	bmean  = sums[0] / npts;
	bsigma = sqrt(sums[1] / npts - bmean * bmean);

	// compute correlation strength and check against minimum. Note that
	// the LAS 4.x Version of the code contains an error... it often tries to
	// access pkval array element zero when there are no significant subsidiary peaks, resulting in
	// unpredictable results

	if (n7 == 0)
		streng = 2 * ((pkval[1] - bmean) / bsigma) - 0.2;
	else
	{
		streng = (pkval[1] -bmean) / bsigma + (pkval[0] - pkval[ipt7]) / bsigma + 0.2 * (n7 - 1.0);
	}

	if (streng < csmin)
	{
		iacrej = 4;
		return;
	}

	//convert 5 by 5 neighbourhood of peak to standard deviations above mean
	krbase = ncol * (ipkrow[1] - 3);
	iptr = 1;
	int I = 1;

	while (I <= 5)
	{
		j = ipkcol[1]-2;
	
		while (j <= ipkcol[1]+2)
		{
			cpval[iptr] = (CCNORM[krbase+j] - bmean) / bsigma;
			iptr++;
			j++;
		}

		krbase += ncol;
		I++;
	}

	return;
}


//---------------------------------------------------------
void CGrid_IMCORR::gnorm(std::vector<double>& CCNORM, std::vector<double>& pkval, std::vector<int>& ipkcol, std::vector<int>& ipkrow, std::vector<double>& sums,
                         std::vector<double> ser, std::vector<double> ref, std::vector<double> UNORMC)
{
	CCNORM.push_back(0.0);

	std::vector<double> xval, colsum, colsqr;
	xval.resize(33);
	colsum.resize(257, 0.0);
	colsqr.resize(257, 0.0);

	std::vector<int> iptr, ixcol, ixrow;
	iptr.resize(33);
	ixcol.resize(33);
	ixrow.resize(33);

	std::vector<int> nsnew(2, (int)sqrt(ser.size() - 1));		// - 1 as we have added dummy zero index
	std::vector<int> nrnew(2, (int)sqrt(ref.size() - 1));
	
	int ipfree, ipt, jstart, jstop, k, kol, koladd, kolsub, line, lnadd, lnsub, ncol, nrow, nrtot;
	double refsum, refsqr, rho, rmean, rsigma, rtotal, sigma1, sigmas, srchsm, srchsq, temp, tempmn, igl, iglnew, iglold;
	refsum = 0;
	refsqr = 0;
	nrtot  = nrnew[0] * nrnew[1];

	ipt = 1;

	while (ipt <= nrtot)
	{
		igl    = ref[ipt];
		refsum = refsum + igl;
		refsqr = refsqr + igl * igl;
		ipt++;
	}
	rtotal = nrtot;
	tempmn = 0.01 * (rtotal * rtotal);
	rmean  = refsum / rtotal;

	if ((refsqr/rtotal - rmean*rmean) > 0.01)
		rsigma = sqrt(refsqr/rtotal - rmean*rmean);
	else
		rsigma = sqrt(0.01);

	// clear sums and sums of squares of normalized correlation values
	sums.push_back(0.0);
	sums.push_back(0.0);
	k = 1;

	while (k <= 32)
	{
		xval[k] = -1.0;
		iptr[k] = k;
		k++;
	}
	ncol = nsnew[0] - nrnew[0] + 1;
	nrow = nsnew[1] - nrnew[1] + 1;

	// compute normalized cross-corr values for one row of alignments at a time
	jstart = 1;
	jstop = ncol;
	int i = 1;

	while (i <= nrow)
	{
		// get column sums and sums of squares for portion of search
		// subimage overlain by reference in current row of alignments
		if (i == 1)
		{
			kol = 1;
			while (kol <= nsnew[0])
			{
				colsum[kol] = 0.0;
				colsqr[kol] = 0.0;
				kol++;
			}

			ipt = 1;
			line = 1;
			while (line <= nrnew[1])
			{
				kol = 1;

				while (kol <= nsnew[0])
				{
					igl         =  ser[ipt];
					colsum[kol] += igl;
					colsqr[kol] += (igl*igl);
					ipt++;
					kol++;
				}
				line++;
			}
		}
		else // if i != 1
		{
			lnsub = (i-2)*nsnew[0];
			lnadd = lnsub + nsnew[0] * nrnew[1];
			kol   = 1;
			while (kol <= nsnew[0])
			{
				iglnew       = ser[lnadd+kol];
				iglold       = ser[lnsub+kol];
				colsum[kol] += (iglnew-iglold);
				colsqr[kol] += ((iglnew*iglnew)-(iglold*iglold));
				kol++;
			}
		}

		// complete computation of search-subarea pixel statistics
		int j = jstart;
		while (j <= jstop)
		{
			
			if (j == jstart)
			{
				srchsm = 0.0;
				srchsq = 0.0;
				kol = 1;
				while (kol <= nrnew[0])
				{
					srchsm += colsum[kol];
					srchsq += colsqr[kol];
					kol++;
				}

				if (tempmn > (rtotal*srchsq - srchsm*srchsm))
					temp = tempmn;
				else
					temp = (rtotal*srchsq - srchsm*srchsm);
				sigmas = sqrt(temp);
			}
			else
			{
				kolsub  = j -jstart;
				koladd  = kolsub + nrnew[0];
				srchsm += (colsum[koladd] - colsum[kolsub]);
				srchsq += (colsqr[koladd] - colsqr[kolsub]);

				if (tempmn > (rtotal*srchsq - srchsm*srchsm))
					temp = tempmn;
				else
					temp = (rtotal*srchsq - srchsm*srchsm);

				sigma1 = 0.5 * (sigmas + temp/sigmas);
				sigmas = 0.5 * (sigma1 + temp/sigma1);
			}

			// compute normalized cross-correlation value
			rho = (UNORMC[j] - rmean*srchsm)/(rsigma * sigmas);
			CCNORM.push_back(rho);
			sums[0]+= rho;
			sums[1]+= (rho*rho);

			// check whether value is among top 32
			if (rho > xval[iptr[32]])
			{
				k = 32;
				ipfree = iptr[32];
				while ( k > 1 && rho > xval[iptr[k-1]])
				{
					iptr[k] = iptr[k-1];
					k       = k-1;
				}
				iptr[k]       = ipfree;
				xval[ipfree]  = rho;
				ixcol[ipfree] = j - jstart + 1;
				ixrow[ipfree] = i;
			}
			j++;
		}

		jstart += ncol;
		jstop  += ncol;
		i++;
	}

	// copy peak values and coordinates in correct sequence
	k = 1;
	pkval.push_back(0.0);
	ipkcol.push_back(0);
	ipkrow.push_back(0);

	while (k <= 32)
	{
		pkval.push_back(xval[iptr[k]]);
		ipkcol.push_back(ixcol[iptr[k]]);
		ipkrow.push_back(ixrow[iptr[k]]);
		k++;
	}

	return;
}


//---------------------------------------------------------
void CGrid_IMCORR::cross(std::vector<double>& UNORMC, std::vector<double> ser, std::vector<double> ref_unpadded)
{
	std::vector<int> nsnew(2, (int)sqrt(ser.size() - 1));			// - 1 as we have added dummy zero index
	std::vector<int> nrnew(2, (int)sqrt(ref_unpadded.size() - 1));

	std::vector<double> ref(ser.size(), 0.);

	int pad = nsnew[0] - nrnew[0];
	int k = 0;

	for (size_t i=0; i<nrnew[0]; i++)
	{
		for (size_t j=0; j<nrnew[1]; j++)
		{
			k++;
			ref[k] = ref_unpadded[i * nrnew[0] + j + 1];
		}

		k += pad;
	}

	// pseudo complex arrays (every second entry empty)
	std::vector<double> cser(ser.size() * 2 - 1, 0.);
	std::vector<double> cref(ser.size() * 2 - 1, 0.);

	for (size_t i=1, j=1; i<ser.size(); i++, j+=2)
	{
		cser[j] = ser[i];
		cref[j] = ref[i];
	}

	// take fast fourier transform of search and reference data
	fft2(cser, nsnew, 1); // from "house" format to frequency format
	fft2(cref, nsnew, 1);

	// make point by multiplication of ft of search ft with conjugate of reference image
	for(size_t i=1; i<cser.size(); i+=2)
	{
		// cser[i] = cser[i] * conjg(cref[i])
		double temp_cser_real = cser[i];
		cser[i]   = cser[i]*cref[i] - cser[i+1]*(-cref[i+1]); 
		cser[i+1] = temp_cser_real*(-cref[i+1]) + cref[i]*(cser[i+1]); 
	}

	// take inverse fft of cser
	fft2(cser, nsnew, -1); // real signal format (house) again

	// extract useful valid correlation
	int ncol = nsnew[0] - nrnew[0] +1;
	int nrow = nsnew[1] - nrnew[1] +1;
	int denom = nsnew[0] * nsnew[1];
	int ndxout = 1;
	int i =1;

	UNORMC.push_back(0.0); // shift for fortran compatibility;

	while(i <= nrow)
	{
		int j = 1;
		while(j <= ncol*2)
		{
			UNORMC.push_back((double)(cser[(i-1)*(nsnew[1]*2)+j])/(double) (denom));

			ndxout++;
			j += 2; // because of pseudo complex configuration
		}

		i += 1;
	}

	return;
}


//---------------------------------------------------------
void CGrid_IMCORR::fft2(std::vector<double>& data, std::vector<int>nel, int isign)
{
	double wr,wi,wpr,wpi,wtemp,theta;
	int ntot = nel[0]*nel[1];

	int nprev = 1;
	int idim = 1;

	while(idim <= 2)
	{
		int n = nel[idim-1];
		int nrem = ntot/(n*nprev);
		int ip1 = 2*nprev;
		int ip2 = ip1*n;
		int ip3 = ip2 * nrem;
		int i2rev = 1;

		// perform bit reversal
		int i2 = 1;
		while(i2 <= ip2)
		{
			if(i2 < i2rev)
			{
				int i1 = i2;
				while (i1 <= (i2+ip1-2))
				{
					int i3 = i1;
					while (i3 <= ip3)
					{
						int i3rev    = i2rev+i3-i2;
						double tempr = data[i3];
						double tempi = data[i3+1];

						data[i3]      = data[i3rev];
						data[i3+1]    = data[i3rev+1];
						data[i3rev]   = tempr;
						data[i3rev+1] = tempi;
						i3 += ip2;
					}
					i1+=2;
				}
			}
			int ibit = ip2/2;
			while (ibit >= ip1 && i2rev > ibit)
			{
				i2rev = i2rev-ibit;
				ibit  = ibit/2;
			}
			i2rev = i2rev+ibit;
			i2 += ip1;
		}
		// end of bit reversal


		// Danielson-Lanczos section
		int ifp1 = ip1;
		while (ifp1 < ip2)
		{
			int ifp2 = 2 * ifp1;
			// initialize for the trigonometric recurrence
			theta = isign *(M_PI*2) / (ifp2/ip1);
			wtemp = sin(0.5*theta);
			wpr   = -2.0 * (wtemp*wtemp);
			wpi   = sin(theta);
			wr    = 1.0;
			wi    = 0.0;
			int i3 = 1;

			while (i3 <= ifp1)
			{
				int i1 = i3;
				while (i1 <= (i3+ip1-2))
				{
					int i2 = i1;
					while (i2 <= ip3)
					{
						int k1 = i2;
						int k2 = k1+ifp1;
						double tempr = wr * data[k2] - wi * data[k2+1];
						double tempi = wr * data[k2+1] + wi * data[k2];
						data[k2]   = data[k1]-tempr;
						data[k2+1] = data[k1+1]-tempi;
						data[k1]   = data[k1]+tempr;
						data[k1+1] = data[k1+1]+tempi;
						i2 += ifp2;
					}
					i1 += 2;
				}
				// the trigonometric recurrence
				wtemp = wr;
				wr    = wr*wpr-wi*wpi+wr;
				wi    = wi*wpr+wtemp*wpi+wi;
				i3   += ip1;
			}
			ifp1 = ifp2;
		}
		nprev = n * nprev;
		idim++;
	}
	
	return;
}


//---------------------------------------------------------
int CGrid_IMCORR::decimal(std::vector<int> Bin) 
{
	int Sum = 0;
	for (int i=0; i<(int)Bin.size(); i++)
	{	
		int Exponent = ((int)Bin.size() -1) - i;
		Sum += Bin[i] * (int)pow(2.0, Exponent);
	}

	return Sum;
}


//---------------------------------------------------------
void CGrid_IMCORR::binary(std::vector<int>& Bin, int number) 
{
	int remainder;

	if(number <= 1)
	{
		Bin.push_back(number);
		return;
	}
	remainder = number%2;
	binary(Bin, number >> 1);    
	Bin.push_back(remainder);
}


//---------------------------------------------------------
void CGrid_IMCORR::Get_Chip(std::vector<double> &chip, CSG_Grid *pGrid, int ix, int iy, int size)
{
	// prepare chip serialized as required for further computations

	int size_2 = size / 2; 

	chip.resize(size * size + 1, 0.);	// + 1 to allow fortran conform indexing (starts at 1)
	int i = 1;

	for (int x=ix-(size_2-1); x<=ix+size_2; x++)
	{
		for (int y =iy-(size_2-1); y<=iy+size_2; y++)
		{
			chip[i] = pGrid->asDouble(x, y);
			i++;
		}
	}

	return;
}


//---------------------------------------------------------
void CGrid_IMCORR::_Add_Fields(CSG_Shapes *pShapes, bool b3D, bool bIDm)
{
	pShapes->Add_Field("ID"           , SG_DATATYPE_Long);
	pShapes->Add_Field("GX"           , SG_DATATYPE_Double);
	pShapes->Add_Field("GY"           , SG_DATATYPE_Double);
	pShapes->Add_Field("REALX"        , SG_DATATYPE_Double);
	pShapes->Add_Field("REALY"        , SG_DATATYPE_Double);
	if (b3D)
	{
		pShapes->Add_Field("REALZ"    , SG_DATATYPE_Double);
	}
	pShapes->Add_Field("DISP"         , SG_DATATYPE_Double);
	if (b3D)
	{
		pShapes->Add_Field("DISP_REAL", SG_DATATYPE_Double);
	}
	pShapes->Add_Field("STRENGTH"     , SG_DATATYPE_Double);
	pShapes->Add_Field("FLAG"         , SG_DATATYPE_Int);
	pShapes->Add_Field("XDISP"        , SG_DATATYPE_Double);
	pShapes->Add_Field("YDISP"        , SG_DATATYPE_Double);
	if (b3D)
	{
		pShapes->Add_Field("ZDISP"    , SG_DATATYPE_Double);
	}
	pShapes->Add_Field("XDISP_UNIT"   , SG_DATATYPE_Double);
	pShapes->Add_Field("YDISP_UNIT"   , SG_DATATYPE_Double);
	pShapes->Add_Field("XTARG"        , SG_DATATYPE_Double);
	pShapes->Add_Field("YTARG"        , SG_DATATYPE_Double);
	if (b3D)
	{
		pShapes->Add_Field("ZTARG"    , SG_DATATYPE_Double);
	}
	pShapes->Add_Field("XERR"         , SG_DATATYPE_Double);
	pShapes->Add_Field("YERR"         , SG_DATATYPE_Double);
	pShapes->Add_Field("ASPECT"       , SG_DATATYPE_Double);
	if (b3D)
	{
		pShapes->Add_Field("SLOPE"    , SG_DATATYPE_Double);
	}
	if (bIDm)
	{
		pShapes->Add_Field("ID_M"     , SG_DATATYPE_Long);
	}

	return;
}


//---------------------------------------------------------
void CGrid_IMCORR::_Add_Shape(CSG_Shapes *pShapes, TSG_Shape_Type ShapeType, bool b3D, bool bIDm, sLong iShape, RESULT res)
{
	CSG_Shape *pShape = pShapes->Add_Shape();

	if (ShapeType == SHAPE_TYPE_Point)
	{
		pShape->Add_Point(res.xReal, res.yReal);
		if (b3D)
		{
			pShape->Set_Z(res.zReal);
		}
	}
	else  // lines
	{
		pShape->Add_Point(res.xReal , res.yReal );
		pShape->Add_Point(res.xReal2, res.yReal2);
		if (b3D)
		{
			pShape->Set_Z(res.zReal , 0);
			pShape->Set_Z(res.zReal2, 1);
		}
	}

	int iField = 0;

	pShape->Set_Value(iField++, iShape);
	pShape->Set_Value(iField++, res.x);
	pShape->Set_Value(iField++, res.y);
	pShape->Set_Value(iField++, res.xReal);
	pShape->Set_Value(iField++, res.yReal);
	if (b3D)
	{
		pShape->Set_Value(iField++, res.zReal);
	}
	pShape->Set_Value(iField++, res.disp);
	if (b3D)
	{
		pShape->Set_Value(iField++, res.dispReal);
	}
	pShape->Set_Value(iField++, res.strength);
	pShape->Set_Value(iField++, res.success);
	pShape->Set_Value(iField++, res.xDisp);
	pShape->Set_Value(iField++, res.yDisp);
	if (b3D)
	{
		pShape->Set_Value(iField++, res.zReal2 - res.zReal);
	}
	pShape->Set_Value(iField++, res.DirNormX);
	pShape->Set_Value(iField++, res.DirNormY);
	pShape->Set_Value(iField++, res.xReal2);
	pShape->Set_Value(iField++, res.yReal2);
	if (b3D)
	{
		pShape->Set_Value(iField++, res.zReal2);
	}
	pShape->Set_Value(iField++, res.xerr);
	pShape->Set_Value(iField++, res.yerr);
	pShape->Set_Value(iField++, res.Aspect);
	if (b3D)
	{
		pShape->Set_Value(iField++, res.Slope);
	}
	if (bIDm)
	{
		pShape->Set_Value(iField++, res.id_m);
	}

	return;
}
