// Copyright (C) 2005 Johan Hoffman.
// Licensed under the GNU GPL Version 2.
//
// Modified by Garth N. Wells 2005.
// Modified by Anders Logg 2005-2006.
//
// First added:  2005
// Last changed: 2006-08-08


// TODO: 
// - change BC to DirichletBC
// - change get/set to vector get/set, i.e. get/set(real* x)
// - Point no longer exists, use real* x

// PETSc friendly version


#include <dolfin/timing.h>
#include <unicorn/FSISolver.h>
#include <unicorn/FSIMomentum3D.h>
#include <unicorn/FSIMomentum2D.h>
#include <unicorn/FSIContinuity3D.h>
#include <unicorn/FSIContinuity2D.h>
#include <unicorn/FSIDotSigma3D.h>
#include <unicorn/FSIDotSigma2D.h>

#include <fstream>

using namespace dolfin;
using namespace dolfin::unicorn;


real VectorMin(const Vector& v) {

  real current_min = FLT_MAX;

  const real* z = v.vec().array();
    
  for (dolfin::uint ndx = 0; ndx < v.size(); ndx++) {
	
    current_min = (current_min > z[ndx]) ? z[ndx] : current_min;
  }

  return current_min;
}


//-----------------------------------------------------------------------------
void FSISolver::ComputePhi(Vector& phi)
{
  phi.init(mesh.numCells());
  phi.zero();

  bisect.interpolate(phi_vxvalues);

  real* z = phi.vec().array();
  
  for (CellIterator cell(mesh); !cell.end(); ++cell) {
    for (VertexIterator n(*cell); !n.end(); ++n) {
      
      //z[cell->index()] += bisect.eval(n->x());
      z[cell->index()] += phi_vxvalues[n->index()];
      if (z[cell->index()] > 0) break;
    }
  }

  cout << "cphi: " << endl;
  phi.disp();

}
/* void FSISolver::ComputePhi(Vector& phi) */
/* { */
/* 	real* z; */

/* 	phi.init(mesh.numCells()); */
/* 	phi.zero(); */

/* 	phi.get(z);			// is this right? We now only */
/* 					// operate on reals ? */


  
/* 	for (CellIterator cell(mesh); !cell.end(); ++cell) { */
/* 		for (VertexIterator n(cell); !n.end(); ++n) { */
      
/* 			phi(cell->index()) += bisect(n->point(), n->index()); */
      
/* 			if (phi(cell->index()) > 0) break; */
/* 		} */
/* 	} */
/* } */
//-----------------------------------------------------------------------------
void FSISolver::ComputeAlpha(Vector& alpha, Vector& phi, real& nu)
{
  alpha.init(mesh.numCells());

  real* a = alpha.vec().array();
  real* p = phi.vec().array();

  for (CellIterator cell(mesh); !cell.end(); ++cell)
    a[cell->index()] = p[cell->index()]*2*nu + (1-p[cell->index()])*k*E;

  cout << "alpha: " << endl;
  alpha.disp();
  cout << "phi: " << endl;
  phi.disp();

}

//-----------------------------------------------------------------------------
/* void FSISolver::ComputeAlpha(Vector& alpha, Vector& phi, real& nu) */
/* { */
/* 	alpha.init(mesh.numCells()); */
  
/* 	for (CellIterator cell(mesh); !cell.end(); ++cell) */
/* 		alpha(cell->index()) = phi(cell->index())*2*nu + (1-phi(cell->index()))*k*E; */
/* } */
//-----------------------------------------------------------------------------
void FSISolver::ComputeDensity(Vector& rho, Vector& phi)
{
  rho.init(mesh.numCells());

  real* r = rho.vec().array();
  real* p = phi.vec().array();

  for (CellIterator cell(mesh); !cell.end(); ++cell)
    r[cell->index()] = p[cell->index()]*rhof + (1-p[cell->index()])*rhos;

}
/* void FSISolver::ComputeDensity(Vector& rho, Vector& phi) */
/* { */
/* 	rho.init(mesh.numCells()); */

/* 	for (CellIterator cell(mesh); !cell.end(); ++cell) */
/* 		rho(cell->index()) = phi(cell->index())*rhof + (1-phi(cell->index()))*rhos; */
/* } */
//-----------------------------------------------------------------------------
FSISolver::FSISolver(Mesh& mesh, 
		     Function& f,
		     Function& ic, 
		     DirichletBC& bc_mom, 
		     DirichletBC& bc_con, 
		     Function& bcf_mom, 
		     Function& bcf_con, 
		     Function& bisect, 
		     real& rhof, 
		     real& rhos, 
		     real& E, 
		     real& elnu, 
		     real& nu,
		     real& T,
		     real& k,
		     real& C_cfl,
		     real& C_k)
  : mesh(mesh), f(f), ic(ic), bc_mom(bc_mom), bc_con(bc_con),
    bcf_mom(bcf_mom), bcf_con(bcf_con),
    bisect(bisect), rhof(rhof), rhos(rhos), E(E), elnu(elnu),
    nu(nu), T(T), k(k), C_cfl(C_cfl), C_k(C_k), assembler(mesh),
    masked_cells(mesh), smoothed_cells(mesh), h0(mesh),
    mqual(0), smoother(mesh)
{
  // Declare parameters for parameter study
  add("velocity file name", "velocity.pvd");
  add("pressure file name", "pressure.pvd");
  add("con residual file name", "residual_con.pvd");
  add("mom residual file name", "residual_mom.pvd");

  boundary = new BoundaryMesh(mesh, boundary_vertex_map,
			      boundary_cell_map);
  
  vertex_is_interior.init(mesh, 0);
  
  for (VertexIterator v(mesh); !v.end(); ++v)
    vertex_is_interior.set(v->index(), true);
  
  for (VertexIterator bv(*boundary); !bv.end(); ++bv)   
    vertex_is_interior.set(boundary_vertex_map(*bv), false);

  int d = mesh.topology().dim();

  phi_vxvalues = new real[mesh.numVertices()];
  u_vxvalues = new real[d * mesh.numVertices()];
}
//-----------------------------------------------------------------------------
FSISolver::~FSISolver() 
{
  delete boundary;
  if(mqual != 0)
    delete mqual;
  delete [] phi_vxvalues;
  delete [] u_vxvalues;
}
//-----------------------------------------------------------------------------
void FSISolver::solve()
{

  real T0 = 0.0;        // start time 
  real t  = 0.0;        // current time
 
  real hmin;
  GetMinimumCellSize(mesh, hmin);
    
  // Lame variable - lambda is not required
  real mu = E / (2 * (1 + elnu));

  cout << "FSIc: FSI Solver has started =======================" << endl;
  cout << "FSIc: ==============================================" << endl;
  cout << "FSIc: time step size k:  " << k << endl;
  cout << "FSIc: minimum cell size: " << hmin << endl;
  cout << "FSIc: nr. of cells:      " << mesh.numCells() << endl;
  cout << "FSIc: nr. of vertices:   " << mesh.numVertices() << endl;
  cout << "FSIc: nr. of space dims: " << mesh.topology().dim() << endl;
  cout << "FSIc: Youngs modulus:    " << E << endl;
  cout << "FSIc: Poissons ratio:    " << elnu << endl;
  cout << "FSIc: Fluid density:     " << rhof << endl;
  cout << "FSIc: Structure density: " << rhos << endl;
  cout << "FSIc: Fluid viscosity:   " << nu << endl;
  cout << "FSIc: ==============================================" << endl;


  // Initialize output files 
  File file_u(get("velocity file name"));  // file for saving velocity 
  File file_p(get("pressure file name"));  // file for saving pressure
  File file_res_con(get("con residual file name"));  // file for saving velocity 
  File file_res_mom(get("mom residual file name"));  // file for saving pressure
  File file_mesh("mixer_mesh.xml");
  File file_phi_func("phi.pvd");


  // Create matrices and vectors 
  Matrix Amom, Acon;
  Vector bmom, bcon;

  // Get the number of space dimensions of the problem 
  unsigned int nsd = mesh.topology().dim();

  // Initialize vectors for velocity and pressure 
  Vector x0vel; // x0vel: velocity from previous time step 
  Vector xcvel; // xcvel: linearized velocity 
  Vector xmvel; // xmvel: cell mean velocity 
  Vector xvel;  // xvel:  current velocity 
  Vector xpre;      // pvel:  current pressure 
  Vector mvel_vec;  // mesh velocity
  Vector mmvel_vec; // cell mean mesh velocity
  Vector cvel_vec;  // convective velocity. Not sure if used
  Vector residual_mom;		// momemtum residual
  Vector residual_con;		// continuity residual
  Vector sigma_vec;		// stress vector

  Vector phi_vec;      // material marker
  Vector rho_vec;      // density of material 
  Vector alpha_vec;    // stress component
  Vector dsig;         // dot sigma, change of stress deviatoric
  Vector icell_vol;    // store the inverse cell volume

  Vector d1vector;
  Vector d2vector;

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

	
  Function u0;			// velocity from previous time step 
  Function uc;			// velocity linearized convection 
  Function um;			// cell mean velocity 
  Function p;			// current pressure
  Function w;
  Function wm;
  Function delta1; 
  Function delta2;
  Function phi;
  Function rho;
  Function alpha;
  Function icv;
  Function sigma;
  Function bound_check_func;
  Function u;			// current velocity
  Function res_con;
  Function res_mom;

  Function dt(mesh, k);		// Constant Functions can be initialized straight away
  Function miu(mesh, mu);		// 
  //Function c;
  Function phi_func;

  Form* adsig = 0;
  Form* Ldsig = 0;
  Form* amom  = 0; 
  Form* Lmom  = 0; 
  Form* acon  = 0; 
  Form* Lcon  = 0;  


  if (nsd == 3) {
    
    Ldsig = new FSIDotSigma3DLinearForm(uc, icv, miu);
    amom  = new FSIMomentum3DBilinearForm(uc,um,delta1,delta2,w,wm,
					  phi,rho,alpha,dt);
    Lmom  = new FSIMomentum3DLinearForm(um,u0,f,p,delta1,delta2,wm,
					phi,rho,sigma,dt);
    acon  = new FSIContinuity3DBilinearForm(delta1);
    Lcon  = new FSIContinuity3DLinearForm(uc,um,f,delta1);   
    
  } else if (nsd == 2) {

    Ldsig = new FSIDotSigma2DLinearForm(uc, icv, miu);
    amom  = new FSIMomentum2DBilinearForm(uc,um,delta1,delta2,w,wm,
					  phi,rho,alpha,dt);
    Lmom  = new FSIMomentum2DLinearForm(um,u0,f,p,delta1,delta2,wm,
					phi,rho,sigma,dt);
    acon  = new FSIContinuity2DBilinearForm(delta1);
    Lcon  = new FSIContinuity2DLinearForm(uc,um,f,delta1);
  
  } else {
    error("FSI solver only implemented for 2 and 3 space dimensions.");
  }
	
  // Initialize Vectors ///////////////////////////////////////////////////////////////////////

  // Initialize Functions /////////////////////////////////////////////////////////////////////

  u.init(mesh, xvel, *amom, 1);   // velocity
  u0.init(mesh, x0vel, *Lmom, 2);   // velocity from previous time step 
  uc.init(mesh, xcvel, *amom, 2);   // velocity linearized convection 
  um.init(mesh, xmvel, *amom, 3);   // velocity linearized convection 
  p.init(mesh, xpre, *Lmom, 4);   // current pressure
  w.init(mesh, mvel_vec, *amom, 6);   // mesh velocity function
  wm.init(mesh, mmvel_vec, *amom, 7);   // mesh velocity function
  delta1.init(mesh, d1vector, *Lmom, 5);   // stabilization parameter
  delta2.init(mesh, d2vector, *Lmom, 6);   // stabilization parameter
  phi.init(mesh, phi_vec,	*Lmom, 8);   // marker
  rho.init(mesh, rho_vec,	*Lmom, 9);   // density
  alpha.init(mesh, alpha_vec, *amom, 10);   // stress component
  icv.init(mesh, icell_vol, *Ldsig, 2);   // inverse cell volume
  sigma.init(mesh, sigma_vec, *Lmom, 10);  // stress 
  res_con.init(mesh, residual_con, *acon,	1);   // residuals
  res_mom.init(mesh, residual_mom, *amom,	1);

  //bound_check_func.init(mesh, bound_check, *Lmom, 4);

  //c.init(mesh,  cvel_vec, *amom)	;
  //phi_func.init(phi_vec, mesh, phi.element.init());

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

  masked_cells.init(mesh.topology().dim());
  smoothed_cells.init(mesh.topology().dim());
  
  // Initialize mesh functions
  for (CellIterator c(mesh); !c.end(); ++c)
  {
    Cell& cell = *c;
    masked_cells.set(cell.index(), false);
  }

  h0.init(mesh.topology().dim());
  
  // Initialize mesh functions
  for (CellIterator c(mesh); !c.end(); ++c)
  {
    Cell& cell = *c;
    h0.set(cell, MeshQuality::myDiameter(cell));
  }
  
  // HACK MESHFUNCTION ///////////////////////////////////////////////////

  bool existRegionFile = false;
  std::fstream fin;
  fin.open("region_output_stitched.xml.gz", std::ios::in);

  if( fin.is_open() ) {
    cout << "FSIc: XML region file exists and will be used." << endl;
    existRegionFile = true;
  } 

  fin.close();

  if (existRegionFile) {

    MeshFunction<bool> fs(mesh);
    
    File fs_in("region_output_stitched.xml.gz");
    fs_in >> fs;

    phi_vec.init(mesh.numCells());
    phi_vec.zero();

    real* z = phi_vec.vec().array();
  
    for (CellIterator c(mesh); !c.end(); ++c) {
      Cell& cell = *c;
      
      if(fs.get(cell))
	z[cell.index()] = 0.0;
      else
	z[cell.index()] = 1.0;
    }

//     for (CellIterator cell(mesh); !cell.end(); ++cell) {
//       for (VertexIterator n(*cell); !n.end(); ++n) {
      
// 	z[cell->index()] += fs(*n);
      
// 	if (z[cell->index()] > 0) break;
//       }
//     }

  } else {

    //Initialization of variables
    ComputePhi(phi_vec);
  }

  // HACK MESHFUNCTION ///////////////////////////////////////////////////


  real* phi_arr = phi_vec.vec().array();

  for (CellIterator cell(mesh); !cell.end(); ++cell)
  {
    if(phi_arr[cell->index()] == 0.0)
    {
      masked_cells.set(cell->index(), true);
      cout << "masking cell: " << cell->index() << endl;
    }
  }



  x0vel.zero();
  xcvel.zero();
  xvel.zero();
  xpre.zero();
  mvel_vec.zero();
  //cvel_vec.zero();
  sigma_vec.zero();

  SetInitialVelocity(xvel, ic);
  ComputeAlpha(alpha_vec, phi_vec, nu);
  ComputeDensity(rho_vec, phi_vec);

  UpdateDotSigma(dsig, icell_vol, *Ldsig);

  // Set residual vectors to 1.0e3 ////////////////////////////////////////////////////////////

  real* r_mom = residual_mom.vec().array();
  real* r_con = residual_con.vec().array();
	
  for (dolfin::uint ndx = 0; ndx < mesh.numVertices(); ndx++) {
    for (dolfin::uint dim = 0; dim < nsd; dim++)
      r_mom[ndx + dim] = 1.0e3;
	  
    r_con[ndx] = 1.0e3;
  }

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




  // Initialize algebraic solvers /////////////////////////////////////////////////////////////
	
  KrylovSolver solver_con(gmres, amg);
  KrylovSolver solver_mom(gmres);

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



	

  // Synchronize Variables ////////////////////////////////////////////////////////////////////
	
//   u.sync(t);
//   p.sync(t);
//   bc_con.sync(t);
//   bc_mom.sync(t);

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


  // Initialize time-stepping parameters
  int time_step  = 0;
  int sample     = 0;
  int no_samples = 200;

  int iteration;
  int max_iteration = 50;

  int stepsize_method = 0;
  if(k == 0.0)
    ComputeStepSize(k, u.vector(), stepsize_method);
  cout << "k: " << k << endl;

  // HACK Swenox FSI convergence problem /////////////////////////////

  int  restarts = 0;
  real k_factor = 1;
  real d_factor = 1;
  bool fpi_success = true;

  // HACK Swenox FSI convergence problem /////////////////////////////


  // Residual, tolerance and maxmimum number of fixed point iterations
  real residual;
  real rtol = 1.0e-2;

  printf("FSIc: time\t itn\t mom.res\t con.res\t tot.res\t el.time \n");
  
  mqual = new MeshQuality(mesh);


  while (t < T) {
    
    tic();
      
    // Mesh diagnostics
      
    uint inverted_cell;

    if(mqual->isInverted(inverted_cell)) {
      cout << "FSIc: inverted cell: " << inverted_cell << endl;
    }

    real mu_max, mu_min, mu_avg;
    real h_max, h_min, h_avg;
     
    mqual->meshQuality(mu_min, mu_max, mu_avg, h_min, h_max, h_avg);
      
    cout << "FSIc: Mesh quality:" << endl;
    cout << "FSIc: mu_min: " << mu_min << endl;
    cout << "FSIc: mu_max: " << mu_max << endl;
    cout << "FSIc: mu_avg: " << mu_avg << endl;
    cout << "FSIc: h_min: " << h_min << endl;
    cout << "FSIc: h_max: " << h_max << endl;
    cout << "FSIc: h_avg: " << h_avg << endl;
      
    //       if(mu_min < 0.2)
    //       {
    // 	file_mesh << mesh;
    // 	file_phi_func << phi_func;
    // 	dolfin_error("mesh quality too bad");
    //       }      

    time_step++;

    if (fpi_success) {
      // Set current velocity to velocity at previous time step 
      x0vel.vec() = xvel.vec();
    }

    // Compute stabilization parameters
    ComputeStabilization(mesh,u0,nu,k,d1vector,d2vector,t);


    // Initialize residual 
    residual = 2*rtol;
    iteration = 0;
		
    // Assemble continuity matrix
    set("output destination", "silent");
    assembler.assemble(Acon, *acon);
    set("output destination", "terminal");
		
    // Fix-point iteration for non-linear problem 
    while (residual > rtol && iteration < max_iteration) {
      
      set("output destination", "silent");
      // Assemble continuity vector 
      assembler.assemble(bcon, *Lcon);

      // Set boundary conditions for continuity equation 
      bc_con.apply(Acon, bcon, *acon);
      set("output destination", "terminal");

      // Stop recording boundary points for reference frame
      //bcf_con.endRecording();

      tic();
      solver_con.solve(Acon, xpre, bcon);
      message("Solve con took %g seconds",toc());

//       cout << "bcon: " << endl;
//       bcon.disp();
//       cout << "xpre: " << endl;
//       xpre.disp();
		
      tic();
      set("output destination", "silent");
      assembler.assemble(Amom, *amom); 
      assembler.assemble(bmom, *Lmom); 
      bc_mom.apply(Amom, bmom, *amom); // TODO
      set("output destination", "terminal");
      message("Assemble mom took %g seconds",toc());

      // Stop recording boundary points for reference frame
      //bcf_mom.endRecording();				// TODO
			
      // Solve the linear system for the momentum equation 
      tic();
      solver_mom.solve(Amom, xvel, bmom);
      message("Solve mom took %g seconds",toc());

//       cout << "bmom: " << endl;
//       bmom.disp();
//       cout << "xvel: " << endl;
//       xvel.disp();


      // Set linearized velocity to current velocity 
      xcvel.vec() = xvel.vec();

      // Compute cell mean
      ComputeMean(mesh,um,uc);
      ComputeMean(mesh,wm,w);

      //cvel_vec = xcvel;
      //cvel_vec -= mvel_vec;
      //cvel_vec = xcvel - mvel_vec;
    
      tic();
      set("output destination", "silent");
      assembler.assemble(Amom, *amom);
      assembler.assemble(bmom, *Lmom);
      bc_mom.apply(Amom, bmom, *amom);  // TODO: use DirichletBC.apply()
      set("output destination", "terminal");
      message("Assemble mom took %g seconds",toc());

      // Compute residual for momentum equation 
      Amom.mat().mult(xvel.vec(),residual_mom.vec());
      residual_mom.vec() -= bmom.vec();
      
      tic();
      set("output destination", "silent");
      assembler.assemble(bcon, *Lcon);
      bc_con.apply(Acon, bcon, *acon);  // TODO
      set("output destination", "terminal");
      message("Assemble con took %g seconds",toc());

      // Compute residual for continuity equation 
      Acon.mat().mult(xpre.vec(),residual_con.vec());
      residual_con.vec() -= bcon.vec();

      residual = sqrt(sqr(residual_mom.norm()) + sqr(residual_con.norm()));

      cout << "iter: " << iteration << endl;
      cout << "res: " << residual << endl;

      iteration++;

	    
    }
    
    printf("FSIc: %3.6f\t %d\t %2.9f\t %2.9f\t %2.9f\t %6.5f \n",
	   t, iteration, residual_mom.norm(), residual_con.norm(), residual, toc());


	


    if (residual > rtol) {
      cout << "FSIc: fixed point iteration did not converge" << endl;
      iteration = 0;

      if (restarts % 2 == 0) {
	k_factor = k_factor / 2.0;
	k = (C_k * k_factor) * VectorMin(d1vector);
      } else {
	d_factor = 2.0;
	C_cfl = (C_cfl * d_factor);
      }
	    
      restarts++;

      fpi_success = false;

      cout << "FSIc: FPI failed. Now using C_k=" << (C_k*k_factor) << ", and C_cfl="<< C_cfl << endl;

    } else {
	    
      fpi_success = true;

      t += k;
	    
      mvel_vec.zero();
      UpdateStructure(mvel_vec, phi_vec, uc);  // move the structure according to uc
	    
      //for (int ii = 0; ii < 0; ii++)
      //UpdateFluid(mvel_vec, phi_vec);          // smooth the fluid mesh

      SmoothMesh();

      mvel_vec.vec() /= k;

      // Update stress deviatoric
      dsig.vec().mult(k);
      sigma_vec.vec() += dsig.vec();
      UpdateDotSigma(dsig, icell_vol, *Ldsig);
	    
	
      //k = C_k * VectorMin(d1vector);

      cout << "FSIc: FPI succeeded using C_k=" << (C_k*k_factor) << ", and C_cfl="<< C_cfl << endl;

    }
		
    if ((time_step == 1) || (t > (T-T0)*(real(sample)/real(no_samples))) ) {
      cout << "saving solution to file" << endl;
	   
      file_p << p;
      file_u << u;

//       file_res_con  << res_con;
//       file_res_mom  << res_mom;
      file_phi_func << phi;

      sample++;
    }
    
//     cout << "time_step: " << time_step << endl;
//     if(time_step > 2)
//       break;
  }

  cout << "saving solution to file" << endl;
  file_p << p;
  file_u << u;

  delete amom;
  delete Lmom;
  delete acon;
  delete Lcon;
  delete adsig;
  delete Ldsig;
}
//-----------------------------------------------------------------------------
void FSISolver::solve(Mesh& mesh, 
		      Function& f, 
		      Function& ic,
		      DirichletBC& bc_mom, 
		      DirichletBC& bc_con, 
		      Function& bcf_mom, 
		      Function& bcf_con, 
		      Function& bisect, 
		      real& rhof,
		      real& rhos, 
		      real& E, 
		      real& elnu, 
		      real& nu,
		      real& T,		      
		      real& k,
		      real& C_cfl,
		      real& C_k)
{
  FSISolver solver(mesh, f, ic, bc_mom, bc_con, bcf_mom, bcf_con,
		   bisect, rhof, rhos, E, elnu, nu, T, k, C_cfl, C_k);
  solver.solve();
}
//-----------------------------------------------------------------------------
void FSISolver::ComputeCellSize(Mesh& mesh, Vector& hvector)
{
  // Compute cell size h

  hvector.init(mesh.numCells());	

  real* h = hvector.vec().array();

  for (CellIterator cell(mesh); !cell.end(); ++cell) {
    h[(*cell).index()] = (*cell).diameter();
  }
}
//-----------------------------------------------------------------------------
void FSISolver::GetMinimumCellSize(Mesh& mesh, real& hmin)
{
  // Get minimum cell diameter
  hmin = 1.0e6;
  for (CellIterator cell(mesh); !cell.end(); ++cell) {
    if ((*cell).diameter() < hmin) hmin = (*cell).diameter();
  }
}
//-----------------------------------------------------------------------------
void FSISolver::ComputeStabilization(Mesh& mesh, Function& w, real nu, real k, 
				     Vector& d1vector, Vector& d2vector, real& t)
{
//   // Compute least-squares stabilizing terms: 
//   //
//   // if  h/nu > 1 or ny < 10^-10
//   //   d1 = C1 * ( 0.5 / sqrt( 1/k^2 + |U|^2/h^2 ) )   
//   //   d2 = C2 * h 
//   // else 
//   //   d1 = C1 * h^2  
//   //   d2 = C2 * h^2  

  real C1 = 4.0;   
  real C2 = 2.0;   

//   d1vector.init(mesh.numCells());	
//   d2vector.init(mesh.numCells());	

  real vel;
  real vel_mean;

  int nsd = mesh.topology().dim(); 

  real* d1arr = d1vector.vec().array();
  real* d2arr = d2vector.vec().array();
  real* warr = w.vector().vec().array();

  for (CellIterator cell(mesh); !cell.end(); ++cell)
    {
      vel_mean = 0.0;
      for (VertexIterator n(*cell); !n.end(); ++n)
      {
	vel = 0.0;
	for ( int i = 0; i < nsd; i++ )
	  vel += sqr(warr[i*mesh.numVertices() + (*n).index()]); 
	
	vel = sqrt(vel);
	vel_mean += vel;
      }
      vel_mean /= (*cell).numEntities(0);
  
      real h = (*cell).diameter();
      //real h = MeshQuality::myDiameter(*cell);

      uint cid = (*cell).index();
      
      if (((h/nu) > 1.0) || (nu < 1.0e-10) ){
	d1arr[cid] = C1 * (0.5 / sqrt( 1.0/sqr(k) + sqr(vel_mean/h)));
	d2arr[cid] = C2 * h;
      } else{
	d1arr[cid] = C1 * sqr(h);
	d2arr[cid] = C2 * sqr(h);
      }	
    }

  d1vector.vec().restore(d1arr);
  d2vector.vec().restore(d2arr);



//   real C1 = 4.0;   
//   real C2 = 2.0;

//   d1vector.init(mesh.numCells());	
//   d2vector.init(mesh.numCells());

//   real* wvector = w.vector().vec().array();

//   real* d1 = d1vector.vec().array();
//   real* d2 = d2vector.vec().array();

//   real normw; 
//   real vel_max = FLT_MIN;
//   real vel;
//   real vel_mean;

//   int nsd = mesh.topology().dim(); 

//   for (CellIterator cell(mesh); !cell.end(); ++cell) {
//     normw = 0.0;
//     vel_mean = 0.0;
//     for (VertexIterator n(*cell); !n.end(); ++n) {
//       vel = 0;
//       for ( int i = 0; i < nsd; i++ ) 
// 	vel += sqr(wvector[i*mesh.numVertices() + (*n).index()]);
	    
//       vel = sqrt(vel);//10.0;//sqrt(vel);
	    
//       vel_mean += vel;
	
//       vel_max = std::max(vel_max, vel); 
//     }
	
//     vel_mean /= cell->numEntities(0);
	
//     normw = cell->diameter() / vel_max;
//     //normw += sqrt( sqr((w.vector())(i*mesh.numVertices() + (*n).index())) ); 
    
//     //normw /= (*cell).numEntities(0);
//     // 	if ( (((*cell).diameter()/nu) > 1.0) || (nu < 1.0e-10) ){
//     //d1vector((*cell).index()) = (t == 0.0)? (C1 * (0.5 / sqrt( 1.0/sqr(k) + sqr(vel_mean/(*cell).diameter()) ) )) : C_cfl * normw;
//     d1[(*cell).index()] = (true)? (C1 * (0.5 / sqrt( 1.0/sqr(k) + sqr(vel_mean/(*cell).diameter()) ) )) : C_cfl * normw;
//     d2[(*cell).index()] = C2 * (*cell).diameter();
		
//     // TODO: cannot access vector by index, i.e. no more get/set
		
//     // 	} else {
//     //  d1vector((*cell).index()) = (t == 0.0)? C1 * sqr((*cell).diameter()) : C_cfl * normw;
//     // 	    d2vector((*cell).index()) = C2 * sqr((*cell).diameter());

//     //     d1vector((*cell).index()) = C1 * sqr((*cell).diameter());
//     // 	    d2vector((*cell).index()) = C2 * sqr((*cell).diameter());
//     // 	}	
//   }

//   //cout << "FSIc: d1vector max is " << VectorMax(d1vector) << ", min is " << VectorMin(d1vector) << endl;
// //   cout << "d1: " << endl;
// //   d1vector.disp();
// //   cout << "d2: " << endl;
// //   d2vector.disp();



}
//-----------------------------------------------------------------------------
void FSISolver::SetInitialVelocity(Vector& xvel, Function& ic)
{
  // Function for setting initial velocity, 
  // given as a function of the coordinates (x,y,z).
  //
  // This function is only temporary: initial velocity 
  // should be possible to set in the main-file. 


  //  real x,y,z;

  real* x = xvel.vec().array();

  for (VertexIterator vertex(mesh); !vertex.end(); ++vertex) {

    // 	for (uint ndx = 0; ndx < mesh.topology().dim(); ndx++) {
	    
    // 	    xvel.set(getij(vertex->index(), ndx), ic(vertex->point(), ndx));
	    
    // 	}   // Get coordinates of the vertex 
    //    real x = (*vertex).coord().x;
    //    real y = (*vertex).coord().y;
    //    real z = (*vertex).coord().z;
    
    // Specify the initial velocity using (x,y,z) 
    //
    // Example: 
    // 
    // xvel((*vertex).index()) = sin(x)*cos(y)*sqrt(z);
    
    x[(*vertex).index()] = 0.0;  }
}
//-----------------------------------------------------------------------------
void FSISolver::ComputeMean(Mesh& mesh, Function& vmean, Function& v)
{
  int nsd = mesh.topology().dim(); 

  real* varr = v.vector().vec().array();
  real* vmeanarr = vmean.vector().vec().array();

  for (CellIterator c(mesh); !c.end(); ++c)
  {
    Cell& cell = *c;

    for ( int i = 0; i < nsd; i++ )
    {
      real cellmean = 0.0;
      for (VertexIterator n(cell); !n.end(); ++n)
	cellmean += varr[i*mesh.numVertices() + (*n).index()];
	  
      cellmean /= cell.numEntities(0);
      
      vmeanarr[i*mesh.numCells() + cell.index()] = cellmean;
    }
  }
}
//-----------------------------------------------------------------------------
void FSISolver::SmoothMesh()
{
  // Elastic smoothing

  real mu_max, mu_min, mu_avg;
  real h_max, h_min, h_avg;

  mqual->meshQuality(mu_min, mu_max, mu_avg, h_min, h_max, h_avg);
  
  cout << "FSIc: Mesh quality before:" << endl;
  cout << "FSIc: mu_min: " << mu_min << endl;
  cout << "FSIc: mu_max: " << mu_max << endl;
  cout << "FSIc: mu_avg: " << mu_avg << endl;
  cout << "FSIc: h_min: " << h_min << endl;
  cout << "FSIc: h_max: " << h_max << endl;
  
  // Initialize mesh functions
  for (CellIterator c(mesh); !c.end(); ++c)
  {
    Cell& cell = *c;
    smoothed_cells.set(cell.index(), true);
  }
  
  for (CellIterator c(mesh); !c.end(); ++c)
  {
    Cell& cell = *c;
    
    if(masked_cells.get(cell))
      smoothed_cells.set(cell, false);
  }
  
  smoother.smooth(smoothed_cells, h0);
  
  mqual->meshQuality(mu_min, mu_max, mu_avg, h_min, h_max, h_avg);
  
  cout << "FSIc: Mesh quality after:" << endl;
  cout << "FSIc: mu_min: " << mu_min << endl;
  cout << "FSIc: mu_max: " << mu_max << endl;
  cout << "FSIc: mu_avg: " << mu_avg << endl;
  cout << "FSIc: h_min: " << h_min << endl;
  cout << "FSIc: h_max: " << h_max << endl;
  cout << "FSIc: h_avg: " << h_avg << endl;
}
//-----------------------------------------------------------------------------
