Index: /issm/trunk-jpl/src/m/contrib/gravity/vfsa.cpp
===================================================================
--- /issm/trunk-jpl/src/m/contrib/gravity/vfsa.cpp	(revision 18445)
+++ /issm/trunk-jpl/src/m/contrib/gravity/vfsa.cpp	(revision 18446)
@@ -134,4 +134,5 @@
 void filtergrav(Matrix* A,Matrix* Ain,double ctr,double sd,int mx,int my);
 void newmodelgen(Matrix* m0,Matrix* m1,Matrix* bathy,Matrix* icethick,int mx,int my,double T,double ptval,double mmax,double mmax2,double ctr,double sd);
+double coolshed(double T0,int k,double c,double D);
 /*}}}*/
 
@@ -197,4 +198,19 @@
 	double ca=0.9;                    /* for acceptance */
 	double cm=0.5;                    /* for model perturbation */
+
+	double T0a          = 0.1;      /* initial temperature for acceptance           */
+	double T0m          = 0.9;      /* initial temperature for model perturbation   */
+	double D            = 2;        /* dimension of the model                       */
+	int    maxconsecrej = 1000;     /* max consecutive rejection                    */
+	int    maxsuccess   = 100;      /* max number of success within one temperature */
+	double T_min        = 1e-10;    /* stopping temp                                */
+	double Tred         = 1;
+	double E_min        = -1000000;
+	double E_exp        = 0.0291;   /* expected misfit                              */
+	int    maxiter      = 10000;
+	int    maxtotaliter = 1000000;
+	double Tol          = 1e-10;    /* tolerance on misfit                          */
+	int    sfreq        = 100;
+
 	                     /*}}}*/     
 	/* load the data {{{*/
@@ -259,32 +275,128 @@
 //	mesh_ini->Echo();
                            /*}}}*/
-	double q=0;
-/* q=misfit(mesh_ini,evalid,gobs,dlevel,Pobs,xobs,yobs,Pp,rho1,rho2,dx,dy,dn,nx,ny,mx,my);
-     cout<< q;
-Matrix* G=new Matrix(4,3);
-	Matrix* df=new Matrix(4,1);
-	Matrix* o=new Matrix(4,1);
-	for(int i= 0;i<4;i++){
-		for(int j=0;j<3;j++){
-			if(j==2){G->SetValue(i,j,1);}
-			else{G->SetValue(i,j,i+j);}
-		}
-		df->SetValue(i,0,i*1e-3);
-	}
-		G->SetValue(0,0,1);
-		G->SetValue(0,1,2);
-
-	Matrix* M = NULL;
-	GSLsquarefit(&M,G,df);
-	for(int i= 0;i<4;i++){
-		o->SetValue(i,0,G->GetValue(i,0)*M->GetValue(0,0)+G->GetValue(i,1)*M->GetValue(1,0)+G->GetValue(i,2)*M->GetValue(2,0));}
-
-	G->Echo();
-	df->Echo();
-	M->Echo();
-	o->Echo();*/
-	Matrix* m1=new Matrix(mx*my,3);
-	double T=100;
-newmodelgen(mesh_ini,m1,bathy,icethick,mx,my,T,ptval,mmax,mmax2,ctr,sd);
+		/* VFSA {{{ */
+
+	/* name of the files to save results */
+	std::ofstream savefile1 ("r_140114b.txt");
+	std::ofstream savefile2("m_140114b.txt");
+
+	/* counters initialization */
+	int    success   = 0;
+	int    finished  = 0;
+	int    consec    = 0;
+	double Ta        = T0a;
+	double Tm        = T0m;
+	int    iterT     = 0;   /* iteration within a T      */
+	int    total     = 0;   /* total number of iteration */
+	int    totaliter = 0;
+	int    msave     = 0;
+	double E_new;
+	double E_final;
+	double dE;
+	double P;
+	double rn;
+	Matrix* m_old    = new Matrix(mx *my,3);
+	Matrix* m_min    = new Matrix(mx *my,3);
+	Matrix* m_new    = new Matrix(mx *my,3);
+	m_old->MatrixEqual(mesh_ini);
+	mesh_ini->Echo();
+
+	/* calculate initial misfit */
+	double E_old=misfit(m_old,evalid,gobs,dlevel,Pobs,xobs,yobs,Pp,rho1,rho2,dx,dy,dn,nx,ny,mx,my);
+
+	/* record initial settings */
+	savefile1 << "P     "<< "Ta    "<< "Tm    "<< "Eold  "<< "totaliter "<< "Tred   "<< endl;
+	savefile1 << "nan   "<<  Ta<<"   "<< Tm<<"   "<< E_old<<"     "<< totaliter<<"         "<< Tred <<"  "<< endl;
+	savefile2 << totaliter<< endl;
+	for(int i=0;i<mx*my;i++){
+	savefile2 << m_old->GetValue(i,0)<<"   "<< m_old->GetValue(i,1)<<"   "<< m_old->GetValue(i,2)<<endl;
+	}
+	
+	/* beginning of the loop */
+
+	while(finished==0){
+		
+		iterT++;
+		totaliter++;
+
+		/* stop or reduce T */
+		if(iterT>=maxiter || success>maxsuccess){
+			if(Ta<T_min || total>maxtotaliter || fabs(E_old)<=Tol){
+				finished=1;
+				total+=iterT;
+				break;
+			}
+			else{ /* reduce T */
+				Ta=coolshed(T0a,Tred,ca,D);
+				Tm=coolshed(T0m,Tred,cm,D);
+				total+=iterT;
+				iterT=0;
+				success=1;
+				Tred++;
+				consec=0;
+			}
+		}
+
+		/* update model and calculate energy */
+
+		newmodelgen(m_old,m_new,bathy,icethick,mx,my,Tm,ptval,mmax,mmax2,ctr,sd);  /* new model */
+	   E_new=misfit(m_new,evalid,gobs,dlevel,Pobs,xobs,yobs,Pp,rho1,rho2,dx,dy,dn,nx,ny,mx,my); /* new energy */
+		dE=E_new-E_old;                                        /* energy difference */
+
+		/* acceptance probability */
+
+		P=exp(-dE/Ta);
+
+		/* stop if energy is lower than specified minimum */
+		if (E_new<E_min){
+			m_old->MatrixEqual(m_new);
+			E_old=E_new;
+			break;
+		}
+
+		rn=rand()/double (RAND_MAX);
+
+		/* accept new model or not */
+		if(dE<=0){
+			m_old->MatrixEqual(m_new);
+			E_old=E_new;
+			success++;
+			consec=0;
+			savefile1 << P<<"   "<<  Ta<<"   "<< Tm<<"   "<< E_old<<"     "<< totaliter<<"         "<< Tred <<"  "<< endl;
+			if(Ta<1e-3){
+				savefile2 << totaliter<< endl;
+				for(int i=0;i<mx*my;i++){
+					savefile2 << m_old->GetValue(i,0)<<"   "<< m_old->GetValue(i,1)<<"   "<< m_old->GetValue(i,2)<<endl;
+				}
+			}
+		}
+		else{
+			if(P>rn){
+				m_old->MatrixEqual(m_new);
+				E_old=E_new;
+				success++;
+				consec=0;
+				savefile1 << P<<"   "<<  Ta<<"   "<< Tm<<"   "<< E_old<<"     "<< totaliter<<"         "<< Tred <<"  "<< endl;
+				if(Ta<1e-3){
+					savefile2 << totaliter<< endl;
+					for(int i=0;i<mx*my;i++){
+						savefile2 << m_old->GetValue(i,0)<<"   "<< m_old->GetValue(i,1)<<"   "<< m_old->GetValue(i,2)<<endl;
+					}
+				}
+			}
+			else{
+				consec++;
+			}
+		}
+	}
+
+	m_min->MatrixEqual(m_old);
+	E_final=E_old;
+	savefile1 << "nan"<<"   "<<  "nan"<<"   "<< "nan"<<"   "<< E_final<<"     "<< "nan"<<"         "<< "nan" <<"  "<< endl;
+	savefile2 << " Mesh final"<< endl;
+	for(int i=0;i<mx*my;i++){
+		savefile2 << m_min->GetValue(i,0)<<"   "<< m_min->GetValue(i,1)<<"   "<< m_min->GetValue(i,2)<<endl;
+	}
+			/*}}}*/
 	return 0;
 }/*}}}*/
@@ -577,2 +689,6 @@
 	}
 }/*}}}*/
+double coolshed(double T0,int k,double c,double D){/*{{{*/
+	double T1=T0*exp(-c*pow(k,1/D));
+	return T1;
+}/*}}}*/