/*************************************************************************************/
/*                                                                              */
/*  File:	   MbICP.h                                                      */
/*  Authors:       Luis Montesano and Javier Minguez                            */
/*  Modified:      1/3/2006                                                     */
/*                                                                              */
/*  This library implements the:                                                */
/*										*/
/*	J. Minguez, F. Lamiraux and L. Montesano				*/
/*	Metric-Based Iterative Closest Point, 					*/
/*  Scan Matching for Mobile Robot Displacement Estimation			*/
/*	IEEE Transactions on Roboticics (2006)					*/
/*                                                                                   */
/*************************************************************************************/
/*
 *  This program 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.
 *
 *  This program 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, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include "MbICP.h"
#include "MbICP2.h"
#include "calcul.h"
#include "sp_matrix.h"
#include <stdio.h>
#include <math.h>
#include "percolate.h"

#ifndef M_PI
	#define M_PI 3.14159265358979323846
#endif

// Initial error to compute error ratio
#define BIG_INITIAL_ERROR 1000000.0F

// Debugging flag. Print sm info in the screen.
// #define INTMATSM_DEB


// ---------------------------------------------------------------
// ---------------------------------------------------------------
// Types definition
// ---------------------------------------------------------------
// ---------------------------------------------------------------



// ---------------------------------------------------------------
// ---------------------------------------------------------------
// Variables definition
// ---------------------------------------------------------------
// ---------------------------------------------------------------

float MAXLASERRANGE;


// ************************
// Extern variables

// structure to initialize the SM parameters
TSMparams params;

// Original points to be aligned
Tscan ptosRef;
Tscan ptosNew;

// Structure of the associations before filtering
TAsoc cp_associations[MAXLASERPOINTS];
int cntAssociationsT;

// Filtered Associations
TAsoc cp_associationsTemp[MAXLASERPOINTS];
int cntAssociationsTemp;

// Those points removed by the projection filter
Tscan ptosNoView;

// Current motion estimation
Tsc motion2;


// ************************
// Some precomputations for each scan to speed up
static float refdqx[MAXLASERPOINTS];
static float refdqx2[MAXLASERPOINTS];
static float refdqy[MAXLASERPOINTS];
static float refdqy2[MAXLASERPOINTS];
static float distref[MAXLASERPOINTS];
static float refdqxdqy[MAXLASERPOINTS];


// value of errors
static float error_k1;
static int numConverged;


// ---------------------------------------------------------------
// ---------------------------------------------------------------
// Protos of the functions
// ---------------------------------------------------------------
// ---------------------------------------------------------------

// Function for compatibility with the scans
static void preProcessingLib(Tpfp *laserK, Tpfp *laserK1,
					  Tsc *initialMotion);

// Function that does the association step of the MbICP
static int EStep();

// Function that does the minimization step of the MbICP
static int MStep(Tsc *solucion);

// Function to do the least-squares but optimized for the metric
static int computeMatrixLMSOpt(TAsoc *cp_ass, int cnt, Tsc *estimacion);

// ---------------------------------------------------------------
// ---------------------------------------------------------------
// External functions
// ---------------------------------------------------------------
// ---------------------------------------------------------------

// ************************
// Function that initializes the SM parameters
// ************************

void Init_MbICP_ScanMatching(float max_laser_range,float Bw, float Br,
					  float L, int laserStep,
					  float MaxDistInter,
					  float filter,
					  int ProjectionFilter,
					  float AsocError,
					  int MaxIter, float error_ratio,
					  float error_x, float error_y, float error_t, int IterSmoothConv){

  #ifdef INTMATSM_DEB
	printf("-- Init EM params . . ");
  #endif

  MAXLASERRANGE = max_laser_range;
  params.Bw = Bw;
  params.Br = Br*Br;
  params.error_th=error_ratio;
  params.MaxIter=MaxIter;
  params.LMET=L;
  params.laserStep=laserStep;
  params.MaxDistInter=MaxDistInter;
  params.filter=filter;
  params.ProjectionFilter=ProjectionFilter;
  params.AsocError=AsocError;
  params.errx_out=error_x;
  params.erry_out=error_y;
  params.errt_out=error_t;
  params.IterSmoothConv=IterSmoothConv;

  #ifdef INTMATSM_DEB
	printf(". OK!\n");
  #endif

}


// ************************
// Function that initializes the SM parameters
// ************************

int MbICPmatcher(Tpfp *laserK, Tpfp *laserK1,
				Tsc *sensorMotion, Tsc *solution){

	int resEStep=1;
	int resMStep=1;
	int numIteration=0;

	// Preprocess both scans
	preProcessingLib(laserK,laserK1,sensorMotion);

	while (numIteration<params.MaxIter){

		// Compute the correspondences of the MbICP
		resEStep=EStep();;

		if (resEStep!=1)
			return -1;

		// Minize and compute the solution
		resMStep=MStep(solution);

		if (resMStep==1)
			return 1;
		else if (resMStep==-1)
			return -2;
		else
			numIteration++;
	}

	return 2;

}



// ---------------------------------------------------------------
// ---------------------------------------------------------------
// Inner functions
// ---------------------------------------------------------------
// ---------------------------------------------------------------

// ************************
// Function that does the association step of the MbICP
// ************************

static int EStep()
{
  int cnt;
  int i,J;

  static Tscan ptosNewRef;
  static int indexPtosNewRef[MAXLASERPOINTS];

  int L,R,Io;
  float dist;
  float cp_ass_ptX,cp_ass_ptY,cp_ass_ptD;
  float tmp_cp_indD;

  float q1x, q1y, q2x,q2y,p2x,p2y, dqx, dqy, dqpx, dqpy, qx, qy,dx,dy;
  float landaMin;
  float A,B,C,D;
  float LMET2;

  LMET2=params.LMET*params.LMET;


	// Transform the points according to the current pose estimation

	ptosNewRef.numPuntos=0;
	for (i=0; i<ptosNew.numPuntos; i++){
		transfor_directa_p ( ptosNew.laserC[i].x, ptosNew.laserC[i].y,
			&motion2, &ptosNewRef.laserC[ptosNewRef.numPuntos]);
		car2pol(&ptosNewRef.laserC[ptosNewRef.numPuntos],&ptosNewRef.laserP[ptosNewRef.numPuntos]); 
		ptosNewRef.numPuntos++;
	}

	// ----
	/* Projection Filter */
	/* Eliminate the points that cannot be seen */
	/* Furthermore it orders the points with the angle */

	cnt = 1; /* Becarefull with this filter (order) when the angles are big >90 */
	ptosNoView.numPuntos=0;
	if (params.ProjectionFilter==1){
		for (i=1;i<ptosNewRef.numPuntos;i++){
			if (ptosNewRef.laserP[i].t>=ptosNewRef.laserP[cnt-1].t){ 
				ptosNewRef.laserP[cnt]=ptosNewRef.laserP[i];
				ptosNewRef.laserC[cnt]=ptosNewRef.laserC[i];
				cnt++;
			}
			else{
				ptosNoView.laserP[ptosNoView.numPuntos]=ptosNewRef.laserP[i];
				ptosNoView.laserC[ptosNoView.numPuntos]=ptosNewRef.laserC[i];
				ptosNoView.numPuntos++;
			}
		}
		ptosNewRef.numPuntos=cnt;
	}


	// ----
	/* Build the index for the windows (this is the role of the Bw parameter */
	/* The correspondences are searched between windows in both scans */
	/* Like this you speed up the algorithm */

	L=0; R=0; /* index of the window for ptoRef */
	Io=0; /* index of the window for ptoNewRef */

	if (ptosNewRef.laserP[Io].t<ptosRef.laserP[L].t) {
		if (ptosNewRef.laserP[Io].t + params.Bw < ptosRef.laserP[L].t){
			while (Io<ptosNewRef.numPuntos-1 && ptosNewRef.laserP[Io].t + params.Bw < ptosRef.laserP[L].t) {
				Io++;
			}
		}
		else{
			while (R<ptosRef.numPuntos-1 && ptosNewRef.laserP[Io].t + params.Bw > ptosRef.laserP[R+1].t)
				R++;
		}
	}
	else{
		while (L<ptosRef.numPuntos-1 && ptosNewRef.laserP[Io].t - params.Bw > ptosRef.laserP[L].t)
			L++;
		R=L;
		while (R<ptosRef.numPuntos-1 && ptosNewRef.laserP[Io].t + params.Bw > ptosRef.laserP[R+1].t)
			R++;
	}

	// ----
	/* Look for potential correspondences between the scans */
	/* Here is where we use the windows */

	cnt=0;
	for (i=Io;i<ptosNewRef.numPuntos;i++){

		// Keep the index of the original scan ordering
		cp_associations[cnt].index=indexPtosNewRef[i];

		// Move the window
		while  (L < ptosRef.numPuntos-1 && ptosNewRef.laserP[i].t - params.Bw > ptosRef.laserP[L].t)
			L = L + 1;
		while (R <ptosRef.numPuntos-1 && ptosNewRef.laserP[i].t + params.Bw > ptosRef.laserP[R+1].t)
			R = R + 1;

		cp_associations[cnt].L=L;
		cp_associations[cnt].R=R;

		if (L==R){
			// Just one possible correspondence

			// precompute stuff to speed up
			qx=ptosRef.laserC[R].x; qy=ptosRef.laserC[R].y;
			p2x=ptosNewRef.laserC[i].x;	p2y=ptosNewRef.laserC[i].y;
			dx=p2x-qx; dy=p2y-qy;
			dist=dx*dx+dy*dy-(dx*qy-dy*qx)*(dx*qy-dy*qx)/(qx*qx+qy*qy+LMET2);

			if (dist<params.Br){
				cp_associations[cnt].nx=ptosNewRef.laserC[i].x;
				cp_associations[cnt].ny=ptosNewRef.laserC[i].y;
				cp_associations[cnt].rx=ptosRef.laserC[R].x;
				cp_associations[cnt].ry=ptosRef.laserC[R].y;
				cp_associations[cnt].dist=dist;
				cnt++;
			}
		}
		else if (L<R)
		{
			// More possible correspondences

			cp_ass_ptX=0;
			cp_ass_ptY=0;
			cp_ass_ptD=100000;

			/* Metric based Closest point rule */
			for (J=L+1;J<=R;J++){

				// Precompute stuff to speed up
				q1x=ptosRef.laserC[J-1].x; q1y=ptosRef.laserC[J-1].y;
				q2x=ptosRef.laserC[J].x; q2y=ptosRef.laserC[J].y;
				p2x=ptosNewRef.laserC[i].x; p2y=ptosNewRef.laserC[i].y;

				dqx=refdqx[J-1]; dqy=refdqy[J-1];
				dqpx=q1x-p2x;  dqpy=q1y-p2y;
				A=1/(p2x*p2x+p2y*p2y+LMET2);
				B=(1-A*p2y*p2y);
				C=(1-A*p2x*p2x);
				D=A*p2x*p2y;

				landaMin=(D*(dqx*dqpy+dqy*dqpx)+B*dqx*dqpx+C*dqy*dqpy)/(B*refdqx2[J-1]+C*refdqy2[J-1]+2*D*refdqxdqy[J-1]);

				if (landaMin<0){ // Out of the segment on one side
					qx=q1x; qy=q1y;}
				else if (landaMin>1){ // Out of the segment on the other side
					qx=q2x; qy=q2y;}
				else if (distref[J-1]<params.MaxDistInter) { // Within the segment and interpotation OK
					qx=(1-landaMin)*q1x+landaMin*q2x;
					qy=(1-landaMin)*q1y+landaMin*q2y;
				}
				else{ // Segment too big do not interpolate
					if (landaMin<0.5){
						qx=q1x; qy=q1y;}
					else{
						qx=q2x; qy=q2y;}
				}

				// Precompute stuff to see if we save the association
				dx=p2x-qx;
				dy=p2y-qy;
				tmp_cp_indD=dx*dx+dy*dy-(dx*qy-dy*qx)*(dx*qy-dy*qx)/(qx*qx+qy*qy+LMET2);

				// Check if the association is the best up to now
				if (tmp_cp_indD < cp_ass_ptD){
					cp_ass_ptX=qx;
					cp_ass_ptY=qy;
					cp_ass_ptD=tmp_cp_indD;
				}
			}

			// Association compatible in distance (Br parameter)
			if (cp_ass_ptD< params.Br){
				cp_associations[cnt].nx=ptosNewRef.laserC[i].x;
				cp_associations[cnt].ny=ptosNewRef.laserC[i].y;
				cp_associations[cnt].rx=cp_ass_ptX;
				cp_associations[cnt].ry=cp_ass_ptY;
				cp_associations[cnt].dist=cp_ass_ptD;

				cnt++;
			}
		}
		else { // This cannot happen but just in case ...
			cp_associations[cnt].nx=ptosNewRef.laserC[i].x;
			cp_associations[cnt].ny=ptosNewRef.laserC[i].y;
			cp_associations[cnt].rx=0;
			cp_associations[cnt].ry=0;
			cp_associations[cnt].dist=params.Br;
			cnt++;
		}
	}  // End for (i=Io;i<ptosNewRef.numPuntos;i++){

	cntAssociationsT=cnt;

	// Check if the number of associations is ok
	if (cntAssociationsT<ptosNewRef.numPuntos*params.AsocError){
		#ifdef INTMATSM_DEB
			printf("Number of associations too low <%d out of %f>\n",
				cntAssociationsT,ptosNewRef.numPuntos*params.AsocError);
		#endif
		return 0;
	}

	return 1;
}


// ************************
// Function that does the minimization step of the MbICP
// ************************

static int MStep(Tsc *solucion){

  Tsc estim_cp;
  int i,cnt,res;
  float error_ratio, error;
  float cosw, sinw, dtx, dty, tmp1, tmp2;
  static TAsoc cp_tmp[MAXLASERPOINTS+1];

	// Filtering of the spurious data
	// Used the trimmed versions that orders the point by distance between associations

     if (params.filter<1){

		// Add Null element in array position 0	(this is because heapsort requirement)
		for (i=0;i<cntAssociationsT;i++){
			cp_tmp[i+1]=cp_associations[i];
		}
		cp_tmp[0].dist=-1;
		// Sort array
		heapsort(cp_tmp, cntAssociationsT);
		// Filter out big distances
		cnt=((int)(cntAssociationsT*100*params.filter))/100;
		// Remove Null element
		for (i=0;i<cnt;i++){
			cp_associationsTemp[i]=cp_tmp[i+1];
		}
	 }
	 else{ // Just build the Temp array to minimize
		cnt=0;
		for (i=0; i<cntAssociationsT;i++){
			if (cp_associations[i].dist<params.Br){
				cp_associationsTemp[cnt]=cp_associations[i];
				cnt++;
			}
		}
	}

	cntAssociationsTemp=cnt;

	#ifdef INTMATSM_DEB
		printf("All assoc: %d  Filtered: %d  Percentage: %f\n",
			cntAssociationsT, cntAssociationsTemp, cntAssociationsTemp*100.0/cntAssociationsT);
	#endif

	// ---
	/* Do de minimization Minimize Metric-based distance */
	/* This function is optimized to speed up */

	res=computeMatrixLMSOpt(cp_associationsTemp,cnt,&estim_cp);
	if (res==-1)
		return -1;

	#ifdef INTMATSM_DEB
		printf("estim_cp: <%f %f %f>\n",estim_cp.x, estim_cp.y,estim_cp.tita);
		printf("New impl: <%f %f %f>\n",estim_cp.x, estim_cp.y,estim_cp.tita);
	#endif

	cosw=(float)cos(estim_cp.tita); sinw=(float)sin(estim_cp.tita);
	dtx=estim_cp.x; dty=estim_cp.y;


	// ------
	/* Compute the error of the associations */

	error=0;
	for (i = 0; i<cnt;i++){
		tmp1=cp_associationsTemp[i].nx * cosw - cp_associationsTemp[i].ny * sinw + dtx - cp_associationsTemp[i].rx;tmp1*=tmp1;
		tmp2=cp_associationsTemp[i].nx * sinw + cp_associationsTemp[i].ny * cosw + dty - cp_associationsTemp[i].ry;tmp2*=tmp2;
		error = error+ tmp1+tmp2;
	}

	error_ratio = error / error_k1;

	#ifdef INTMATSM_DEB
		printf("<err,errk1,errRatio>=<%f,%f,%f>\n estim=<%f,%f,%f>\n",
			error,error_k1,error_ratio, estim_cp.x,estim_cp.y, estim_cp.tita);
	#endif

	// ----
	/* Check the exit criteria */
	/* Error ratio */
	if (fabs(1.0-error_ratio)<=params.error_th ||
		(fabs(estim_cp.x)<params.errx_out && fabs(estim_cp.y)<params.erry_out
		&& fabs(estim_cp.tita)<params.errt_out) ){
		numConverged++;
	}
	else
		numConverged=0;

	//--
	/* Build the solution */
	composicion_sis(&estim_cp, &motion2, solucion);
	motion2=*solucion;
	error_k1=error;

	/* Number of iterations doing convergence (smooth criterion of convergence) */
	if (numConverged>params.IterSmoothConv)
		return 1;
	else
		return 0;
}


// ************************
// Function to do the least-squares but optimized for the metric
// ************************

static int computeMatrixLMSOpt(TAsoc *cp_ass, int cnt, Tsc *estimacion) {

	int i;
	float LMETRICA2;
	float X1[MAXLASERPOINTS], Y1[MAXLASERPOINTS];
	float X2[MAXLASERPOINTS],Y2[MAXLASERPOINTS];
	float X2Y2[MAXLASERPOINTS],X1X2[MAXLASERPOINTS];
	float X1Y2[MAXLASERPOINTS], Y1X2[MAXLASERPOINTS];
	float Y1Y2[MAXLASERPOINTS];
	float K[MAXLASERPOINTS], DS[MAXLASERPOINTS];
	float DsD[MAXLASERPOINTS], X2DsD[MAXLASERPOINTS], Y2DsD[MAXLASERPOINTS];
	float Bs[MAXLASERPOINTS], BsD[MAXLASERPOINTS];
	float A1, A2, A3, B1, B2, B3, C1, C2, C3, D1, D2, D3;
	MATRIX matA,invMatA;
	VECTOR vecB,vecSol;

	A1=0;A2=0;A3=0;B1=0;B2=0;B3=0;
	C1=0;C2=0;C3=0;D1=0;D2=0;D3=0;


	LMETRICA2=params.LMET*params.LMET;

	for (i=0; i<cnt; i++){
		X1[i]=cp_ass[i].nx*cp_ass[i].nx;
		Y1[i]=cp_ass[i].ny*cp_ass[i].ny;
		X2[i]=cp_ass[i].rx*cp_ass[i].rx;
		Y2[i]=cp_ass[i].ry*cp_ass[i].ry;
		X2Y2[i]=cp_ass[i].rx*cp_ass[i].ry;

		X1X2[i]=cp_ass[i].nx*cp_ass[i].rx;
		X1Y2[i]=cp_ass[i].nx*cp_ass[i].ry;
		Y1X2[i]=cp_ass[i].ny*cp_ass[i].rx;
		Y1Y2[i]=cp_ass[i].ny*cp_ass[i].ry;

		K[i]=X2[i]+Y2[i] + LMETRICA2;
		DS[i]=Y1Y2[i] + X1X2[i];
		DsD[i]=DS[i]/K[i];
		X2DsD[i]=cp_ass[i].rx*DsD[i];
		Y2DsD[i]=cp_ass[i].ry*DsD[i];

		Bs[i]=X1Y2[i]-Y1X2[i];
		BsD[i]=Bs[i]/K[i];

		A1=A1 + (1-Y2[i]/K[i]);
		B1=B1 + X2Y2[i]/K[i];
		C1=C1 + (-cp_ass[i].ny + Y2DsD[i]);
		D1=D1 + (cp_ass[i].nx - cp_ass[i].rx -cp_ass[i].ry*BsD[i]);

		A2=B1;
		B2=B2 + (1-X2[i]/K[i]);
		C2=C2 + (cp_ass[i].nx-X2DsD[i]);
		D2=D2 + (cp_ass[i].ny -cp_ass[i].ry +cp_ass[i].rx*BsD[i]);

		A3=C1;
		B3=C2;
		C3=C3 + (X1[i] + Y1[i] - DS[i]*DS[i]/K[i]);
		D3=D3 + (Bs[i]*(-1+DsD[i]));
	}


	initialize_matrix(&matA,3,3);
	MDATA(matA,0,0)=A1;	MDATA(matA,0,1)=B1;	MDATA(matA,0,2)=C1;
	MDATA(matA,1,0)=A2;	MDATA(matA,1,1)=B2;	MDATA(matA,1,2)=C2;
	MDATA(matA,2,0)=A3;	MDATA(matA,2,1)=B3;	MDATA(matA,2,2)=C3;

	if (inverse_matrix (&matA, &invMatA)==-1)
		return -1;

#ifdef INTMATSM_DEB
	print_matrix("inverted matrix", &invMatA);
#endif

	initialize_vector(&vecB,3);
	VDATA(vecB,0)=D1; VDATA(vecB,1)=D2; VDATA(vecB,2)=D3;
	multiply_matrix_vector (&invMatA, &vecB, &vecSol);

	estimacion->x=-VDATA(vecSol,0);
	estimacion->y=-VDATA(vecSol,1);
	estimacion->tita=-VDATA(vecSol,2);

	return 1;
}


// ------------------------------------
// Function added by Javi for compatibility
// ------------------------------------

static void preProcessingLib(Tpfp *laserK, Tpfp *laserK1,
					  Tsc *initialMotion)
{

	int i,j;

	motion2=*initialMotion;

	// ------------------------------------------------//
	// Compute xy coordinates of the points in laserK1
	ptosNew.numPuntos=0;
	for (i=0; i<MAXLASERPOINTS; i++) {
                if (laserK1[i].r <MAXLASERRANGE){
			ptosNew.laserP[ptosNew.numPuntos].r=laserK1[i].r;
			ptosNew.laserP[ptosNew.numPuntos].t=laserK1[i].t;
			ptosNew.laserC[ptosNew.numPuntos].x=(float)(laserK1[i].r * cos(laserK1[i].t));
			ptosNew.laserC[ptosNew.numPuntos].y=(float)(laserK1[i].r * sin(laserK1[i].t));
		    ptosNew.numPuntos++;
		}
        }

	// Choose one point out of params.laserStep points
	j=0;
	for (i=0; i<ptosNew.numPuntos; i+=params.laserStep) {
		ptosNew.laserC[j]=ptosNew.laserC[i];
		j++;
	}
	ptosNew.numPuntos=j;

	// Compute xy coordinates of the points in laserK
	ptosRef.numPuntos=0;
	for (i=0; i<MAXLASERPOINTS; i++) {
 		if (laserK[i].r <MAXLASERRANGE){
			ptosRef.laserP[ptosRef.numPuntos].r=laserK[i].r;
			ptosRef.laserP[ptosRef.numPuntos].t=laserK[i].t;
			ptosRef.laserC[ptosRef.numPuntos].x=(float)(laserK[i].r * cos(laserK1[i].t));
			ptosRef.laserC[ptosRef.numPuntos].y=(float)(laserK[i].r * sin(laserK1[i].t));
		    ptosRef.numPuntos++;
		}
	}

	// Choose one point out of params.laserStep points
	j=0;
	for (i=0; i<ptosRef.numPuntos; i+=params.laserStep) {
		ptosRef.laserC[j]=ptosRef.laserC[i];
		j++;
	}
	ptosRef.numPuntos=j;
	// ------------------------------------------------//

	// Preprocess reference points
	for (i=0;i<ptosRef.numPuntos-1;i++) {
		car2pol(&ptosRef.laserC[i],&ptosRef.laserP[i]);
		refdqx[i]=ptosRef.laserC[i].x - ptosRef.laserC[i+1].x;
		refdqy[i]=ptosRef.laserC[i].y - ptosRef.laserC[i+1].y;
		refdqx2[i]=refdqx[i]*refdqx[i];
		refdqy2[i]=refdqy[i]*refdqy[i];
		distref[i]=refdqx2[i] + refdqy2[i];
		refdqxdqy[i]=refdqx[i]*refdqy[i];
	}
	car2pol(&ptosRef.laserC[ptosRef.numPuntos-1],&ptosRef.laserP[ptosRef.numPuntos-1]);

	error_k1=BIG_INITIAL_ERROR;
	numConverged=0;
}
