/*
 * Marblesbag - Copyright (C) <2010-2018>
 * <Universite catholique de Louvain (UCL), Belgium
 *  Universite de Montpellier, France>
 * 	
 * List of the contributors to the development of Marblesbag: see AUTHORS file.
 * Description and complete License: see LICENSE file.
 * 	
 * This program (Marblesbag) is free software: 
 * you can redistribute it and/or modify it under the terms of the GNU Lesser General 
 * Public License as published by the Free Software Foundation, either version
 * 3 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 Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program (see COPYING and COPYING.LESSER files).  If not, 
 * see <http://www.gnu.org/licenses/>.
 */

#include "tools.h"
#include <stdlib.h>
#include <stdio.h>
#include <float.h>
#include <math.h>
#include "fluid_problem.h"
#include "mesh_find.h"
#include "mbxml.h"


#define newton_max_it 10
#define newton_atol 5e-7
#define newton_rtol 1e-5

#if DIMENSION==2

#define N_SF 3
#define N_N 3
#define N_QP 3
static double QP[][2] = {{1./6,1./6},{1./6,2./3},{2./3,1./6}};
static double QW[] = {1./6,1./6,1./6};
static void shape_functions(double *uvw, double *sf)
{
  sf[0] = 1-uvw[0]-uvw[1];
  sf[1] = uvw[0];
  sf[2] = uvw[1];
}
static void grad_shape_functions(const double dxidx[2][2], double gsf[3][2])
{
  for (int i = 0;  i < 2; ++i) {
    gsf[0][i] = -dxidx[0][i]-dxidx[1][i];
    gsf[1][i] = dxidx[0][i];
    gsf[2][i] = dxidx[1][i];
  }
}
static double detDD(const double m[2][2])
{
  return m[0][0]*m[1][1] - m[0][1]*m[1][0];
}
static double invDD(const double m[2][2], double inv[2][2])
{
  double d = detDD(m);
  inv[0][0] = m[1][1]/d;
  inv[0][1] = -m[0][1]/d;
  inv[1][0] = -m[1][0]/d;
  inv[1][1] = m[0][0]/d;
  return d;
}
#else

#define N_SF 4
#define N_N 4
#define N_QP 5
static double QP[][3] = {
  {0.25, 0.25, 0.25},
  {0.5, 1./6, 1./6},
  {1./6, 0.5, 1./6},
  {1./6, 1./6, 0.5},
  {1./6, 1./6, 1./6}
};
static double QW[] = {-16./120, 9./120, 9./120, 9./120, 9./120};
static void shape_functions(double *uvw, double *sf)
{
  sf[0] = 1-uvw[0]-uvw[1]-uvw[2];
  sf[1] = uvw[0];
  sf[2] = uvw[1];
  sf[3] = uvw[2];
}
static void grad_shape_functions(const double dxidx[3][3], double gsf[4][3])
{
  for (int i = 0; i < 3; ++i) {
    gsf[0][i] = -dxidx[0][i]-dxidx[1][i]-dxidx[2][i];
    gsf[1][i] = dxidx[0][i];
    gsf[2][i] = dxidx[1][i];
    gsf[3][i] = dxidx[2][i];
  }
};
static double detDD(const double m[3][3])
{
  return m[0][0]*(m[1][1]*m[2][2]-m[2][1]*m[1][2])
        -m[1][0]*(m[0][1]*m[2][2]-m[2][1]*m[0][2])
        +m[2][0]*(m[0][1]*m[1][2]-m[1][1]*m[0][2]);
}
static double invDD(const double m[3][3], double inv[3][3]) {
  double d = detDD(m);
  inv[0][0] = (m[1][1]*m[2][2]-m[2][1]*m[1][2])/d;
  inv[0][1] = -(m[0][1]*m[2][2]-m[2][1]*m[0][2])/d;
  inv[0][2] = (m[0][1]*m[1][2]-m[1][1]*m[0][2])/d;
  inv[1][0] = -(m[1][0]*m[2][2]-m[2][0]*m[1][2])/d;
  inv[1][1] = (m[0][0]*m[2][2]-m[2][0]*m[0][2])/d;
  inv[1][2] = -(m[0][0]*m[1][2]-m[1][0]*m[0][2])/d;
  inv[2][0] = (m[1][0]*m[2][1]-m[2][0]*m[1][1])/d;
  inv[2][1] = -(m[0][0]*m[2][1]-m[2][0]*m[0][1])/d;
  inv[2][2] = (m[0][0]*m[1][1]-m[1][0]*m[0][1])/d;
  return d;
};

#endif

#if DIMENSION==2
static double particle_drag_coeff(double u[2], double mu, double rho, double vol, double c)
{
  double d = 2*sqrt(vol/M_PI);
  double normu = hypot(u[0],u[1]);
  //Reynolds/|u_p-u_f|
  double Re_O_u = sqrt(d)*c/mu*rho;
  double Cd_u = 0.63*sqrt(normu)+4.8/sqrt(Re_O_u);
  Cd_u = Cd_u*Cd_u;
  double f = pow(c,-1.8);
  return f*Cd_u*rho/2*d;
}
#else
static double particle_drag_coeff(double u[3], double mu, double rho, double vol, double c)
{
  double d = 2*cbrt(3./4.*vol/M_PI);
  double normu = sqrt(u[0]*u[0]+u[1]*u[1]+u[2]*u[2]);
  //Reynolds/|u_p-u_f|
  double Re_O_u = d*c/mu*rho;
  double Cd_u = 0.63*sqrt(normu)+4.8/sqrt(Re_O_u);
  Cd_u = Cd_u*Cd_u;
  double f = pow(c,-1.8);
  double r = d/2.;
  return f*Cd_u*rho/2*(M_PI*r*r);
}
#endif

void fluid_problem_compute_node_particle_force(FluidProblem *problem, double dt, double *particle_force) {
  for (int i = 0; i < problem->n_particles*DIMENSION; ++i) {
    particle_force[i] = problem->particle_force[i];
  }
}

static void compute_node_force_implicit(FluidProblem *problem, double dt, double *all_local_vector, double *all_local_matrix) {
  double *compacity = problem->compacity;
  Mesh *mesh = problem->mesh;
  int n_fields = problem->nFields;
  int N_E = problem->mesh->n_elements;
  for (int i = 0; i < problem->n_particles; ++i) {
    int iel = problem->particle_element_id[i];
    double mass = problem->particle_mass[i];
    if(iel < 0){
      for (int d = 0; d < DIMENSION; ++d)
        problem->particle_force[i*DIMENSION+d] = 0;
      problem->particle_force[i*DIMENSION+1] = problem->g*mass;
      continue;
    }
    double *xi = problem->particle_uvw + DIMENSION*i;
    double phi[N_SF];
    shape_functions(xi,phi);
    uint32_t *el=problem->mesh->elements+iel*N_SF;
    double *se = problem->solution_explicit;
    
    double dxdxi[DIMENSION][DIMENSION],dxidx[DIMENSION][DIMENSION];
    for (int k=0; k<DIMENSION; ++k)
      for (int j=0; j<DIMENSION; ++j)
        dxdxi[k][j] = mesh->x[el[j+1]*3+k]-mesh->x[el[0]*3+k];
    invDD(dxdxi,dxidx);
    double dphi[N_SF][DIMENSION];
    grad_shape_functions(dxidx, dphi);
    double *si = problem->solution;
    for (int ff=0;ff<problem->nFluids;++ff){
    double c=0,vf[DIMENSION]={0}, vfe[DIMENSION]={0}, gradp[DIMENSION]={0};
    double beta = 0;
    for (int iphi=0; iphi < N_SF; ++iphi) {
      c += compacity[el[iphi]]*phi[iphi];
      beta += si[el[iphi]*n_fields+DIMENSION+1+ff*(DIMENSION+1)];
      for (int j=0; j < DIMENSION; ++j) {
        vf[j]  += si[el[iphi]*n_fields+j+ff*(DIMENSION+1)]*phi[iphi];
        vfe[j] += se[el[iphi]*n_fields+j+ff*(DIMENSION+1)]*phi[iphi];
        gradp[j] += dphi[iphi][j]*si[el[iphi]*n_fields+n_fields-2];
      }
    }
    double du[DIMENSION], due[DIMENSION];
    for (int j = 0; j < DIMENSION; ++j) {
      du[j] = problem->particle_velocity[i*DIMENSION+j]-vf[j]/(1-c);
      due[j] = problem->particle_velocity[i*DIMENSION+j]-vfe[j]/(1-c);
    }
    double vol = problem->particle_volume[i];
    double rhop = mass/vol;
    double drho = rhop -problem->rho[ff];
    double g = problem->g*drho/rhop;
    double gamma = particle_drag_coeff(due,problem->mu[ff],problem->rho[ff],vol,1-c);
    double fcoeff = beta*mass/(mass+dt*gamma);
    
    for (int d = 0; d < DIMENSION; ++d)
      problem->particle_force[i*DIMENSION+d] = fcoeff*(-gamma*du[d]-vol*gradp[d]);
    problem->particle_force[i*DIMENSION+1] += fcoeff*g*mass;
    

    double *local_vector = &all_local_vector[N_SF*n_fields*iel];
      double *local_matrix = &all_local_matrix[N_SF*N_SF*n_fields*n_fields*iel];
    for (int iphi = 0; iphi < N_SF; ++iphi) {
      for (int d = 0; d < DIMENSION; ++d)
        local_vector[iphi+N_SF*d+ff*(DIMENSION+1)] -= fcoeff*gamma*(du[d]-dt/mass*vol*gradp[d])*phi[iphi];
      local_vector[iphi+N_SF*1+ff*(DIMENSION+1)] -= fcoeff*gamma*dt*g*phi[iphi];
      
    }
    for (int iphi = 0; iphi < N_SF; ++iphi) {
      for (int jphi = 0; jphi < N_SF; ++jphi) {
        int N2 = n_fields*N_SF;
        int IDX = (iphi*N2+jphi)*n_fields;
#define LOCAL_MATRIX(a,b) local_matrix[IDX+N2*a+b]
        double f = fcoeff*phi[iphi];
        for (int d = 0; d < DIMENSION; ++d){
          LOCAL_MATRIX(d+ff*(DIMENSION+1),d+ff*(DIMENSION+1)) -= -beta*f/(1-c)*phi[jphi]*gamma;
          LOCAL_MATRIX(d+ff*(DIMENSION+1),DIMENSION) -= -beta*f*gamma*dt/mass*vol*dphi[jphi][d];
        }
      }
    }
    }
  }
}

static void fluid_problem_assemble_system(FluidProblem *problem, double *rhs, const double *solution_old, double dt)
{
  HXTLinearSystem *lsys = problem->linear_system;
  const Mesh *mesh = problem->mesh;
  const double *solution = problem->solution;
  int n_fields = problem->nFields;
  const double *compacity = problem->compacity;
  const double *old_compacity = problem->old_compacity;
  const double *mu = problem->mu;
  const double *rho = problem->rho;
  const double epsilon = problem->epsilon;
  int n_fluids = problem->nFluids;
  size_t local_size = N_SF*n_fields;
  size_t N_E = mesh->n_elements;
  double *all_local_vector = malloc(N_E*local_size*sizeof(double));
  double *all_local_matrix = malloc(N_E*local_size*local_size*sizeof(double));
  for (size_t i = 0; i < N_E*local_size; ++i)
    all_local_vector[i] = 0;
  for (size_t i = 0; i < N_E*local_size*local_size; ++i)
    all_local_matrix[i] = 0;
  compute_node_force_implicit(problem, dt, all_local_vector, all_local_matrix);
 
  for (int iel=0; iel < mesh->n_elements; ++iel) {
    const unsigned int *el = &mesh->elements[iel*N_N];
    double dxdxi[DIMENSION][DIMENSION], dxidx[DIMENSION][DIMENSION], dphi[N_N][DIMENSION];
    int P = DIMENSION;    
    double *local_vector = &all_local_vector[N_SF*n_fields*iel];
    double c=0,p=0, dp[DIMENSION]={0};
    for (int i = 0; i < DIMENSION; ++i)
        for (int j = 0; j < DIMENSION; ++j)
          dxdxi[i][j] = mesh->x[el[j+1]*3+i] - mesh->x[el[0]*3+i];
    const double detj = invDD(dxdxi, dxidx);
    grad_shape_functions(dxidx, dphi);
    for (int iqp = 0; iqp< N_QP; ++iqp) {
      const double jwl = QW[iqp]*detj;
      double phi[N_SF];
      shape_functions(QP[iqp],phi);
      for (int i = 0; i < N_SF; ++i) {
          double phii = phi[i];
          p += solution[el[i]*n_fields+P]*phi[i];
          c += compacity[el[i]]*phi[i];
          local_vector[i+N_SF*P] += jwl*phii*(c-1);
          for (int j = 0; j < DIMENSION; ++j)
            dp[j]   += dphi[i][j]*solution[el[i]*n_fields+P];
      }
    }//printf("c=%g,p=%g\n",c,p);
    
    double *local_matrix = &all_local_matrix[N_SF*N_SF*n_fields*n_fields*iel];
    for (int ff = 0; ff < n_fluids; ++ff)
    {
      int Q = n_fields-1;//DIMENSION+ff*(DIMENSION+1);
      double dq[DIMENSION]={0}, du[DIMENSION][DIMENSION]={{0}}, dqdq[DIMENSION]={0};
      for (int i = 0; i< N_SF; ++i) {
        for (int j = 0; j < DIMENSION; ++j) {
          dq[j]   += dphi[i][j]*solution[el[i]*n_fields+Q];
          dqdq[j] += dphi[i][j];
          for (int k = 0; k < DIMENSION; ++k)
            du[k][j] += dphi[i][j]*solution[el[i]*n_fields+k+ff*(DIMENSION+1)];
        }
      }
      for (int iqp = 0; iqp< N_QP; ++iqp) {
        double phi[N_SF];
        shape_functions(QP[iqp],phi);
        double u[DIMENSION]={0}, dudt[DIMENSION]={0}, dqdt=0, q=0;
        for (int i = 0; i < N_SF; ++i) {
          for (int j = 0; j < DIMENSION; ++j){
            u[j] += solution[el[i]*n_fields+j+ff*(DIMENSION+1)]*phi[i];
            dudt[j] += (solution[el[i]*n_fields+j+ff*(DIMENSION+1)]-solution_old[el[i]*n_fields+j+ff*(DIMENSION+1)])/dt*phi[i];
          }
          q +=  solution[el[i]*n_fields+Q]*phi[i];
          dqdt += (solution[el[i]*n_fields+Q]-solution_old[el[i]*n_fields+Q])*phi[i]/dt;
          //dcdt += (problem->porosity[el[i]]-problem->old_porosity[el[i]])/dt*phi[i];
        }
          printf("q=%g,dqdt=%g\n",q,dqdt);
        const double jw = QW[iqp]*detj;
        for (int iphi = 0; iphi < N_SF; ++iphi) {
          double phii = phi[iphi];
          const double *dphii = dphi[iphi];
          double tau[DIMENSION][DIMENSION];
          for (int i = 0; i < DIMENSION; ++i)
            for (int j = 0; j < DIMENSION; ++j)
              tau[i][j] = du[i][j]-u[i]*dq[j]/q;
          double dphidp = 0, divu = 0;
          for (int k = 0; k < DIMENSION; ++k){
            dphidp += dphii[k]*dp[k];
            divu += du[k][k];
          }
          for (int j = 0; j < DIMENSION; ++j) {
            double utau = 0;
            double dphisigma=0;
            for (int k = 0; k < DIMENSION; ++k) {
              //utau += u[k]*dphii[k]*u[j]/c;
              utau += u[k]*tau[j][k];
              dphisigma += 2*mu[ff]*dphii[k]*0.5*(tau[k][j]+tau[j][k]);
            }
            local_vector[iphi+N_SF*j] += jw*(
                dphisigma
                //+rho*phii*dudt[j]-rho*utau
                +rho[ff]*phii*(dudt[j]+u[j]/q*divu+utau/q)
                -p*dphii[j]
                );
          }
          local_vector[iphi+N_SF*P] += jw*phii*(q);
          local_vector[iphi+N_SF*Q] += jw*phii*(dqdt+divu)+jw*epsilon*dphidp;
          for (int jphi = 0; jphi < N_SF; ++jphi) {
            double phij = phi[jphi];
            const double *dphij = dphi[jphi];
            //int N2 = N_SF*N_SF;
            //int IDX = iphi*N2+jphi;
            int N2 = n_fields*N_SF;
            int IDX = (iphi*N2+jphi)*n_fields;
  #define LOCAL_MATRIX(a,b) local_matrix[IDX+N2*a+b]
            double dphiidphij = 0;
            double dtau[DIMENSION];
            double udtau = 0;
            double dphiidtau = 0;
            for (int j = 0; j < DIMENSION; ++j) {
              dtau[j] = dphij[j]-phij*dq[j]/q;
              dphiidphij += dphii[j]*dphij[j];
              udtau += u[j]*dtau[j];
              dphiidtau += dphii[j]*dtau[j];
            }
            for (int j = 0; j < DIMENSION; ++j) {
              int U = j+ff*(DIMENSION+1);
              LOCAL_MATRIX(U,P) += -jw*dphii[j]*phij;
              LOCAL_MATRIX(Q,U) +=  jw*phii*dphij[j];
              double utau =0;
              double dphisigmadq = 0;
              double dutaudq = 0;
              for (int k = 0; k < DIMENSION; ++k) {
                int V = k+ff*(DIMENSION+1);
                utau += u[k]*tau[j][k];
                dutaudq += u[k]*u[j]*(dq[k]*phij/(q*q)-dqdq[k]/q);
                dphisigmadq += 2*mu[ff]*dphii[k]*0.5*(u[k]*dq[j]*phij/(q*q)+u[j]*dq[k]*phij/(q*q)-u[k]*dqdq[j]/q-u[j]*dqdq[k]/q);
                //LOCAL_MATRIX(U,V) += -jw*rho*u[j]*dphii[k]*phij/c+jw*2*mu*dphii[k]*dtau[j]*0.5;
                LOCAL_MATRIX(U,V) +=
                   jw*rho[ff]*phii*phij*tau[j][k]/q
                  +jw*2*mu[ff]*dphii[k]*dtau[j]*0.5
                  +jw*rho[ff]*phii*u[j]*dphij[k]/q;
              }
              //LOCAL_MATRIX(U,U) += jw*mu*2*0.5*dphiidtau + jw*rho*(phii*phij/dt-phij*dphii[j]*u[j]/c);
              LOCAL_MATRIX(U,U) += jw*mu[ff]*2*0.5*dphiidtau + jw*rho[ff]*phii*(phij/dt+phij/q*divu+udtau/q);
              LOCAL_MATRIX(U,Q) += jw*rho[ff]*phii*(-u[j]*divu*phij/(q*q)-utau*phij/(q*q)+dutaudq/q)+jw*dphisigmadq;
            }
            LOCAL_MATRIX(Q,P) += jw*epsilon*dphiidphij;
            LOCAL_MATRIX(P,Q) += -jw*phii*phij;
            LOCAL_MATRIX(Q,Q) += jw*phii*phij/dt;
          }
        }
      }
      hxtLinearSystemAddToMatrix(lsys,iel,iel,local_matrix);
      hxtLinearSystemAddToRhs(lsys,rhs,iel,local_vector);
    }
  }
  free(all_local_matrix);
  free(all_local_vector);

  for (int ibnd = 0; ibnd < problem->n_strong_boundaries; ++ibnd) {
    const StrongBoundary *bnd = problem->strong_boundaries + ibnd;
    int iphys;
    for (iphys = 0; iphys < mesh->n_phys; ++iphys) {
      if (strcmp(bnd->tag, mesh->phys_name[iphys]) == 0)
        break;
    }
    if (iphys == mesh->n_phys)
      printf("Boundary tag \"%s\" not found.", bnd->tag);
    for (int i = 0; i < mesh->phys_n_nodes[iphys]; ++i){
      hxtLinearSystemSetMatrixRowIdentity(lsys, mesh->phys_nodes[iphys][i], bnd->field);
      hxtLinearSystemSetRhsEntry(lsys, rhs,mesh->phys_nodes[iphys][i], bnd->field, 0.);
    }
  }
}



static void apply_boundary_conditions(const Mesh *mesh, int n_fields, double *solution, int nBnd, StrongBoundary *bnds)
{
  for (int ibnd = 0; ibnd < nBnd; ++ibnd) {
    StrongBoundary *bnd = bnds + ibnd;
    int iphys;
    for (iphys = 0; iphys < mesh->n_phys; ++iphys) {
      if (strcmp(bnd->tag, mesh->phys_name[iphys]) == 0)
        break;
    }
    if (iphys == mesh->n_phys){
      printf("Boundary tag \"%s\" not found.", bnd->tag);
    }
    double *x = malloc(mesh->phys_n_nodes[iphys]*sizeof(double)*DIMENSION);
    double *v = malloc(mesh->phys_n_nodes[iphys]*sizeof(double));
    for (int i = 0; i < mesh->phys_n_nodes[iphys]; ++i){
      int j = mesh->phys_nodes[iphys][i];
      for (int k = 0; k < DIMENSION; ++k)
        x[i*DIMENSION+k] = mesh->x[j*3+k];
    }
    bnd->apply(mesh->phys_n_nodes[iphys], x, v);
    for (int i = 0; i < mesh->phys_n_nodes[iphys]; ++i)
      solution[mesh->phys_nodes[iphys][i]*n_fields+bnd->field] = v[i];
    free(x);
    free(v);
  }
}

int fluid_problem_implicit_euler(FluidProblem *problem, double dt)
{
  static double totaltime = 0;
  struct timespec tic, toc;
  const Mesh *mesh = problem->mesh;
  apply_boundary_conditions(mesh, problem->nFields, problem->solution, problem->n_strong_boundaries, problem->strong_boundaries);
  int n_fields = problem->nFields;
  double *solution_old = malloc(mesh->n_nodes*n_fields*sizeof(double));
  double *rhs = malloc(mesh->n_nodes*n_fields*sizeof(double));
  for (int i=0; i<mesh->n_nodes*n_fields; ++i){
    solution_old[i] = problem->solution[i];
    printf("old_solution=%g,solution=%g\n",solution_old[i],problem->solution[i]);}
    
  problem->solution_explicit = solution_old;
  double *dx = malloc(sizeof(double)*mesh->n_nodes*n_fields);
  double norm0 = 0;
  int i;
  for (i=0; i<newton_max_it; ++i) {
    double norm = 0;
    hxtLinearSystemZeroMatrix(problem->linear_system);
    for (int i=0; i<mesh->n_nodes*n_fields; ++i)
      rhs[i] = 0;
    fluid_problem_assemble_system(problem, rhs, solution_old, dt);
    //exit(0);
   // if (i==1) break;
    hxtLinearSystemGetRhsNorm(problem->linear_system,rhs,&norm);
    printf("iter %i norm = %g\n", i, norm);
    if (norm < newton_atol)
      break;
    if (i == 0)
      norm0 = norm;
    else if(norm/norm0 < newton_rtol)
      break;
  clock_get(&tic);
    hxtLinearSystemSolve(problem->linear_system, rhs, dx);
  clock_get(&toc);
  totaltime += clock_diff(&tic, &toc);
    for (int j=0; j<mesh->n_nodes*n_fields; ++j) {
      problem->solution[j] -= dx[j];
    }
   /* for (int j=0; j<mesh->n_nodes*n_fields; ++j){
      printf("sol=%g\n",dx[j]);
    }
    exit(0);*/
  }
  printf("total solve time : %g\n", totaltime);
  free(dx);
  free(rhs);
  free(solution_old);
  return i;
}

static void fluid_problem_compute_node_volume(FluidProblem *problem) {
  free(problem->node_volume);
  Mesh *mesh = problem->mesh;
  problem->node_volume = malloc(sizeof(double)*mesh->n_nodes);
  for (int i = 0; i < problem->mesh->n_nodes; ++i){
    problem->node_volume[i] = 0;
  }
  for (int iel = 0; iel < mesh->n_elements; ++iel) {
    double dxdxi[DIMENSION][DIMENSION];
    const int *el = &mesh->elements[iel*N_N];
    for (int i=0; i<DIMENSION; ++i)
      for (int j=0; j<DIMENSION; ++j)
        dxdxi[i][j] = mesh->x[el[j+1]*3+i]-mesh->x[el[0]*3+i];
#if DIMENSION == 2
    const double vol = detDD(dxdxi)/6;
#else
    const double vol = detDD(dxdxi)/24;
#endif
    for (int i = 0; i < N_N; ++i)
      problem->node_volume[el[i]] += vol;
  }
}

static void fluid_problem_compute_porosity(FluidProblem *problem)
{
  Mesh *mesh = problem->mesh;
  double *volume = problem->particle_volume;
  for (int i = 0; i < mesh->n_nodes; ++i){
    problem->compacity[i] = 0;
  }
  for (int i = 0; i < problem->n_particles; ++i) {
    if(problem->particle_element_id[i] == -1) continue;
    double sf[N_SF];
    shape_functions(&problem->particle_uvw[i*DIMENSION],sf);
    const uint32_t *el = &mesh->elements[problem->particle_element_id[i]*N_N];
    for (int j=0; j<N_N;++j)
      problem->compacity[el[j]] += sf[j]*volume[i];
  }
  for (int i = 0; i < mesh->n_nodes; ++i){
    problem->compacity[i] = 0;
  }
}

FluidProblem *fluid_problem_new(const char *mesh_file_name, double g, double *mu, double *rho, double epsilon, int n_fluids, int n_strong_boundaries, StrongBoundary *strong_boundaries)
{
  static int initialized = 0;
  if (!initialized) {
    hxtInitializeLinearSystems(0, NULL);
    initialized = 1;
#ifdef HXT_HAVE_PETSC
    hxtPETScInsertOptions("-pc_type lu -ksp_max_it 30", "fluid");
#endif
  }
  FluidProblem *problem = malloc(sizeof(FluidProblem));
  Mesh *mesh = mesh_load_msh(mesh_file_name);
  if (!mesh)
    return NULL;
  problem->mesh_tree = mesh_tree_create(mesh);
  problem->rho = malloc(n_fluids*sizeof(double));
  problem->mu = malloc(n_fluids*sizeof(double));
  for (int i = 0; i < n_fluids; ++i)
  {
    printf("mu=%g,rho=%g\n",mu[i],rho[i]);
    problem->rho[i] = rho[i];
    problem->mu[i] = mu[i];
  }
  problem->nFluids = n_fluids;
  problem->nFields = (DIMENSION+1)*n_fluids+1;
  int n_fields = problem->nFields;
  problem->g = g;
  problem->mesh = mesh;
  problem->epsilon = epsilon;
  problem->strong_boundaries = malloc(sizeof(StrongBoundary)*n_strong_boundaries);
  problem->n_strong_boundaries = n_strong_boundaries;
  for (int i = 0; i < n_strong_boundaries; ++i) {
    problem->strong_boundaries[i].tag = strdup(strong_boundaries[i].tag);
    problem->strong_boundaries[i].apply = strong_boundaries[i].apply;
    problem->strong_boundaries[i].field = strong_boundaries[i].field;
  }
  problem->compacity = malloc(mesh->n_nodes*sizeof(double));
  problem->old_compacity = malloc(mesh->n_nodes*sizeof(double));
  problem->solution = malloc(mesh->n_nodes*n_fields*sizeof(double));
  for (int i = 0; i < problem->mesh->n_nodes; ++i)
  {
    problem->compacity[i] = 0.;
  }
  // begin to remove
  for (int i = 0; i < problem->mesh->n_nodes*n_fields; ++i){
    problem->solution[i] = 0.;
  }
  problem->node_volume = malloc(0);
  fluid_problem_compute_node_volume(problem);
  // end to remove
  problem->linear_system = NULL;
#ifdef HXT_HAVE_PETSC
    hxtLinearSystemCreatePETSc(&problem->linear_system,mesh->n_elements,N_N,n_fields,mesh->elements,"fluid");
#else
    hxtLinearSystemCreateLU(&problem->linear_system,mesh->n_elements,N_N,n_fields,mesh->elements);
#endif
  problem->n_particles = 0;
  problem->particle_uvw = malloc(0);
  problem->particle_element_id = malloc(0);
  problem->particle_position = malloc(0);
  problem->particle_mass = malloc(0);
  problem->particle_force = malloc(0);
  problem->particle_volume = malloc(0);
  problem->particle_velocity = malloc(0);
  return problem;
}

void fluid_problem_free(FluidProblem *problem)
{
  free(problem->solution);
  free(problem->compacity);
  free(problem->old_compacity);
  hxtLinearSystemDelete(&problem->linear_system);
  free(problem->particle_uvw);
  free(problem->particle_element_id);
  free(problem->particle_position);
  free(problem->particle_mass);
  free(problem->particle_force);
  free(problem->particle_volume);
  free(problem->particle_velocity);
  for (int i = 0; i < problem->n_strong_boundaries; ++i)
    free(problem->strong_boundaries[i].tag);
  free(problem->strong_boundaries);
  mesh_tree_free(problem->mesh_tree);
  mesh_free(problem->mesh);
}

void fluid_problem_adapt_gen_mesh(FluidProblem *problem, double lcmax, double lcmin, double n_el)
{
  double ngradM = 0.;
  Mesh *mesh = problem->mesh;
  double *solution = problem->solution;
  double *new_mesh_size = malloc(sizeof(double)*mesh->n_nodes);
  double *num_lc = malloc(sizeof(double)*mesh->n_nodes);
  for (int i = 0; i < mesh->n_nodes; ++i){
    new_mesh_size[i] = 0.;
    num_lc[i] = 0.;
  }
  double gradmax = 0.;
  double gradPmax = 0.;
  double gradmin = 1e8;
  double gradPmin = 1e8;
  double gradM = 0.;
  double gradPM = 0.;
  double volT = 0.;
  int n_fields = problem->nFields;
  double *compacity = problem->compacity;
  for (int iel = 0; iel < mesh->n_elements; ++iel) {
    const unsigned int *el = &mesh->elements[iel*N_N];
    double C[N_N],U[DIMENSION][N_N], P[N_N];
    for (int i = 0; i < N_N; ++i){
      C[i] = compacity[el[i]];
      P[i] = solution[el[i]*n_fields+DIMENSION];
      for (int j=0; j<DIMENSION; ++j) {
        U[j][i] = solution[el[i]*n_fields+j];
      }
    }
    double dxdxi[DIMENSION][DIMENSION],dxidx[DIMENSION][DIMENSION];
    for (int i=0; i<DIMENSION; ++i)
      for (int j=0; j<DIMENSION; ++j)
        dxdxi[i][j] = mesh->x[el[j+1]*3+i]-mesh->x[el[0]*3+i];
#if DIMENSION == 2
    const double vol = detDD(dxdxi)/2;
#else        
    const double vol = detDD(dxdxi)/6;
#endif    
    invDD(dxdxi, dxidx);
    double dphi[N_SF][DIMENSION];
    grad_shape_functions(dxidx, dphi);
    double ngrad2 = 0;
    for (int i=0; i<DIMENSION; ++i){
      for (int j=0; j<DIMENSION; ++j){
        double gradU = 0;
        for (int k=0; k<N_SF; ++k){
          gradU += dphi[k][j]*U[i][k]/C[k];
        }
        ngrad2 += gradU*gradU;
      }
    }
    gradM += sqrt(ngrad2)*vol;
    volT += vol;
    gradmax = fmax(gradmax,ngrad2);
    gradmin = fmin(gradmin,ngrad2);
    ngrad2 = 0;
    for (int j=0; j<DIMENSION; ++j){
      double gradP = 0;
      for (int k=0; k<N_SF; ++k){
        gradP += dphi[k][j]*P[k];
      }
      ngrad2 += gradP*gradP;
    }
    gradPM += sqrt(ngrad2)*vol;
    gradPmax = fmax(gradPmax,ngrad2);
    gradPmin = fmin(gradPmin,ngrad2);
  }
  gradPM /= volT;
  gradM /= volT;
  gradPmin = 20*sqrt(gradPmin)/gradPM;
  gradPmax = .1*sqrt(gradPmax)/gradPM;
  gradmin = 20*sqrt(gradmin)/gradM;
  gradmax = .1*sqrt(gradmax)/gradM;
  printf("gradPmax=%e\n",gradPmax);
  printf("gradPmin=%e\n",gradPmin);
  for (int iel = 0; iel < mesh->n_elements; ++iel) {
    const unsigned int *el = &mesh->elements[iel*N_N];
    double C[N_N],U[DIMENSION][N_N], P[N_N];
    for (int i = 0; i < N_N; ++i){
      C[i] = compacity[el[i]];
      P[i] = solution[el[i]*n_fields+DIMENSION];
      for (int j=0; j<DIMENSION; ++j) {
        U[j][i] = solution[el[i]*n_fields+j];
      }
    }
    double dxdxi[DIMENSION][DIMENSION],dxidx[DIMENSION][DIMENSION];
    for (int i=0; i<DIMENSION; ++i)
      for (int j=0; j<DIMENSION; ++j)
        dxdxi[i][j] = mesh->x[el[j+1]*3+i]-mesh->x[el[0]*3+i];
    invDD(dxdxi, dxidx);
    double dphi[N_SF][DIMENSION];
    grad_shape_functions(dxidx, dphi);
    double ngrad2 = 0;
    for (int i=0; i<DIMENSION; ++i){
      for (int j=0; j<DIMENSION; ++j){
        double gradU = 0;
        for (int k=0; k<N_SF; ++k){
          gradU += dphi[k][j]*U[i][k]/C[k];
        }
        ngrad2 += gradU*gradU;
      }
    }
/*    double ngrad = pow(ngrad2, 0.25); */
/*    for (int j=0; j<DIMENSION; ++j){*/
/*      double gradP = 0;*/
/*      for (int k=0; k<N_SF; ++k){*/
/*        gradP += dphi[k][j]*P[k];*/
/*      }*/
/*      ngrad2 += gradP*gradP;*/
/*    }*/
    double ngrad = sqrt(ngrad2)/gradM;
    double lc = lcmin /fmin(1,fmax(ngrad/(gradmax),gradmin/(gradmax)));
    
    ngrad2 = 0.; 
    for (int j=0; j<DIMENSION; ++j){
      double gradP = 0;
      for (int k=0; k<N_SF; ++k){
        gradP += dphi[k][j]*P[k];
      }
      ngrad2 += gradP*gradP;
    }
    ngrad = sqrt(ngrad2)/gradPM;
    lc = fmin(fmin(lcmin /fmin(1,fmax(ngrad/(gradPmax),gradPmin/(gradPmax))),lc),lcmax);
    
    
    for (int j = 0; j < N_N; ++j){
      new_mesh_size[el[j]] += lc;
      num_lc[el[j]] += 1.;
    }
  }
  double nOfEl = 0.0;
#if DIMENSION==2
  for (int i = 0; i < mesh->n_nodes; ++i){
    new_mesh_size[i] /= num_lc[i];
    double nOfElByNode = 4*problem->node_volume[i]/(new_mesh_size[i]*new_mesh_size[i]*sqrt(3));
    nOfEl += nOfElByNode;
  }
  double ratioEl = n_el/nOfEl;
  for (int i = 0; i < mesh->n_nodes; ++i){
    new_mesh_size[i] = fmax(new_mesh_size[i]/sqrt(ratioEl),lcmin);
  }
#else
  for (int i = 0; i < mesh->n_nodes; ++i){
    new_mesh_size[i] /= num_lc[i];
    double nOfElByNode = 12*problem->node_volume[i]/(new_mesh_size[i]*new_mesh_size[i]*new_mesh_size[i]*sqrt(2));
    nOfEl += nOfElByNode;
  }
  double ratioEl = n_el/nOfEl;
  printf("RatioEl = %e\n",ratioEl);
  for (int i = 0; i < mesh->n_nodes; ++i){
    new_mesh_size[i] = fmax(new_mesh_size[i]/cbrt(ratioEl),lcmin);
  }
#endif
  FILE *f = fopen("adapt/lc.pos", "w");
  if(!f)
    printf("cannot open file adapt/lc.pos for writing\n");
  fprintf(f, "View \"lc\" {\n");
  for (int iel = 0; iel < mesh->n_elements; ++iel) {
    unsigned int *el = problem->mesh->elements+iel*N_N;
    fprintf(f, DIMENSION == 2 ? "ST(" : "SS(");
    for (int i = 0; i < N_N; ++i)
      fprintf(f, "%.8g, %.8g, %.8g%s", mesh->x[el[i]*3+0], mesh->x[el[i]*3+1],mesh->x[el[i]*3+2], i != N_N-1 ? "," : "){");
    for (int i = 0; i < N_N; ++i)
      fprintf(f, "%.8g%s", new_mesh_size[el[i]],i != N_N-1?",":"};\n");
  }
  fprintf(f,"};\n");
  fclose(f);
  free(new_mesh_size);
  free(num_lc);
  system("gmsh -"xstr(DIMENSION)" adapt/mesh.geo");
}


void fluid_problem_adapt_mesh(FluidProblem *problem, double lcmax, double lcmin, double n_el)
{
  struct timespec tic, toc;
  fluid_problem_adapt_gen_mesh(problem, lcmax, lcmin, n_el);
  clock_get(&tic);
  Mesh *new_mesh = mesh_load_msh("adapt/mesh.msh");
  int n_fields = problem->nFields;
  clock_get(&toc);
  printf("Time mesh_load : %.6e \n",clock_diff(&tic,&toc));
  double *new_solution = malloc(sizeof(double)*new_mesh->n_nodes*n_fields);
  double *new_xi = malloc(sizeof(double)*new_mesh->n_nodes*DIMENSION);
  clock_get(&tic);
  int *new_eid = malloc(sizeof(int)*new_mesh->n_nodes);
  for (int i = 0; i < new_mesh->n_nodes; ++i)
    new_eid[i] = -1;
  clock_get(&toc);
  printf("Time new_eid : %.6e \n",clock_diff(&tic,&toc));
  clock_get(&tic);
  double *newx = malloc(sizeof(double)*DIMENSION*new_mesh->n_nodes);
  for (int i = 0;i < new_mesh->n_nodes;++i) 
    for (int k = 0; k < DIMENSION; ++k)
      newx[i*DIMENSION+k] = new_mesh->x[i*3+k];
  clock_get(&toc);
  printf("Time newx : %.6e \n",clock_diff(&tic,&toc));
  clock_get(&tic);
  mesh_tree_particle_to_mesh(problem->mesh_tree,new_mesh->n_nodes, newx, new_eid, new_xi);
  clock_get(&toc);
  printf("Time find new point in old mesh : %.6e \n",clock_diff(&tic,&toc));
  for (int i = 0; i < new_mesh->n_nodes; ++i) {
    unsigned int *el = problem->mesh->elements+new_eid[i]*N_N;
    if(new_eid[i] < 0) {
      printf("new mesh point not found in old mesh\n");
      exit(1);
    }
    double phi[N_SF];
    shape_functions(new_xi+i*DIMENSION, phi);
    for (int j=0; j<n_fields; ++j) {
      new_solution[i*n_fields+j] = 0;
      for (int k = 0; k < N_SF; ++k)
        new_solution[i*n_fields+j] += problem->solution[el[k]*n_fields+j]*phi[k];
    }
  }
  free(new_eid);
  free(new_xi);
  free(newx);
  free(problem->solution);
  problem->solution = new_solution;
  free(problem->compacity);
  problem->compacity = malloc(sizeof(double)*new_mesh->n_nodes);
  free(problem->old_compacity);
  problem->old_compacity = malloc(sizeof(double)*new_mesh->n_nodes);
  for (int i = 0; i < new_mesh->n_nodes; ++i)
  {
    problem->compacity[i] = 1.;
    problem->old_compacity[i] = 1.;
  }
  mesh_free(problem->mesh);
  problem->mesh = new_mesh;
  mesh_tree_free(problem->mesh_tree);
  problem->mesh_tree = mesh_tree_create(problem->mesh);
  for (int i = 0; i < problem->n_particles; ++i)
    problem->particle_element_id[i] = -1;
  mesh_tree_particle_to_mesh(problem->mesh_tree, problem->n_particles, problem->particle_position, problem->particle_element_id, problem->particle_uvw);
  fluid_problem_compute_node_volume(problem);
  fluid_problem_compute_porosity(problem);
  hxtLinearSystemDelete(&problem->linear_system);
  #ifdef HXT_HAVE_PETSC
    hxtLinearSystemCreatePETSc(&problem->linear_system,new_mesh->n_elements,N_N,n_fields,new_mesh->elements,"fluid");
  #else
    hxtLinearSystemCreateLU(&problem->linear_system,new_mesh->n_elements,N_N,n_fields,new_mesh->elements);
  #endif 
}

void fluid_problem_after_import(FluidProblem *problem)
{
  mesh_tree_free(problem->mesh_tree);
  problem->mesh_tree = mesh_tree_create(problem->mesh);
  free(problem->old_compacity);
  problem->old_compacity = malloc(sizeof(double)*problem->mesh->n_nodes);
  for (int i = 0; i < problem->n_particles; ++i)
    problem->particle_element_id[i] = -1;
  mesh_tree_particle_to_mesh(problem->mesh_tree, problem->n_particles, problem->particle_position, problem->particle_element_id, problem->particle_uvw);
  fluid_problem_compute_node_volume(problem);
  fluid_problem_compute_porosity(problem);
  hxtLinearSystemDelete(&problem->linear_system);
  #ifdef HXT_HAVE_PETSC
    hxtLinearSystemCreatePETSc(&problem->linear_system,problem->mesh->n_elements,N_N,problem->nFields,problem->mesh->elements,"fluid");
  #else
    hxtLinearSystemCreateLU(&problem->linear_system,new_mesh->n_elements,N_N,n_fields,new_mesh->elements);
  #endif 
}


void fluid_problem_set_particles(FluidProblem *problem, int n, double *mass, double *volume, double *position, double *velocity, long int *elid, int init)
{
  
  int n_fields = problem->nFields;
  
  if(problem->n_particles != n) {
    problem->n_particles = n;
    free(problem->particle_uvw);
    free(problem->particle_element_id);
    free(problem->particle_position);
    free(problem->particle_mass);
    free(problem->particle_force);
    free(problem->particle_volume);
    free(problem->particle_velocity);
    problem->particle_uvw = malloc(sizeof(double)*DIMENSION*n);
    problem->particle_element_id = malloc(sizeof(int)*n);
    for (int i = 0; i < n; ++i) {
      problem->particle_element_id[i]=-1;
    }
    problem->particle_position = malloc(sizeof(double)*DIMENSION*n);
    problem->particle_mass = malloc(sizeof(double)*n);
    problem->particle_force = malloc(sizeof(double)*n*DIMENSION);
    problem->particle_volume = malloc(sizeof(double)*n);
    problem->particle_velocity = malloc(sizeof(double)*n*DIMENSION);
  }
  if (elid) {
    for (int i = 0; i < n; ++i) {
      problem->particle_element_id[i]=elid[i];
    }
  }
  mesh_tree_particle_to_mesh(problem->mesh_tree, n, position, problem->particle_element_id, problem->particle_uvw);
  for (int i = 0; i < n; ++i) {
    problem->particle_mass[i] = mass[i];
    problem->particle_volume[i] = volume[i];
    for (int k = 0; k < DIMENSION; ++k) {
      problem->particle_position[i*DIMENSION+k] = position[i*DIMENSION+k];
      problem->particle_velocity[i*DIMENSION+k] = velocity[i*DIMENSION+k];
    }
  }
  for (int i = 0; i < problem->mesh->n_nodes; ++i){
    problem->old_compacity[i] = problem->compacity[i];
  }
  if (init) {
  //initial fluid_0+grains
    int Q = DIMENSION+1+0*(DIMENSION+1);
    for (int i = 0; i < problem->mesh->n_nodes; ++i){
      problem->solution[i*n_fields+Q] = 1-problem->compacity[i];
   //   printf("compacity=%g\n",problem->compacity[i]);
   }
      
  }
  fluid_problem_compute_porosity(problem);
}

int *fluid_problem_particle_element_id(FluidProblem *problem)
{
  return problem->particle_element_id;
}

int fluid_problem_n_particles(FluidProblem *problem)
{
  return problem->n_particles;
}