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

#define n_fields 3

#define newton_max_it 20
#define newton_atol 1e-12
#define newton_rtol 1e-5

#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};

int fluid_problem_export(const FluidProblem *problem, const char *output_dir, double t, int iter)
{
  char file_name[1024];
  sprintf(file_name,"%s/fluid_%05i.msh",output_dir, iter);
  FILE *f = fopen(file_name, "w");
  if (!f){
    printf("Cannot open file \"%s\" for writing.\n", file_name);
    return -1;
  }
  if(mesh_write_msh(problem->mesh, f))
    return -1;
  if(mesh_write_msh_scalar(problem->mesh, f, "porosity", t, iter, problem->porosity, 1, 0))
    return -1;
  if(mesh_write_msh_vector(problem->mesh, f, "uv", t, iter, problem->solution, n_fields, 0, 1))
    return -1;
   if(mesh_write_msh_vector(problem->mesh, f, "force", t, iter, problem->node_force, 2, 0, 1))
    return -1;
  if(mesh_write_msh_scalar(problem->mesh, f, "p", t, iter, problem->solution, n_fields, 2))
    return -1;
  fclose(f);
  return 0;
}

static void particle_drag(double u[2], double mu, double rho, double d, double c, double drag[2], double dt, double mass, double rhop, double gradp[2])
{
  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);
  double GoU = f*Cd_u*rho/2*d;

  double a = 0;
  double h = (1+(1-c)*rhop/(c*rho)+a)/(a+1);
  //printf("h = %.6g\n", h);
  double F = (mass*GoU)/(mass+h*dt*GoU);
  //double F = mass/(h*dt);

  drag[0] = (u[0]-gradp[0]*dt/rhop)*F;
  drag[1] = (u[1]-9.81*dt*(1-rho/rhop)-gradp[1]*dt/rhop)*F; 
}

static double mesh_get_min_edge_length(Mesh *mesh) {
  double emin = DBL_MAX;
  for (int i= 0; i < mesh->n_triangles; ++i) {
    const int *tri = mesh->triangles+i*3;
    for (int j = 0; j < 3; j++) {
      double *x0 = mesh->x + tri[j]*3;
      double *x1 = mesh->x + tri[(j+1)%3]*3;
      double dx[] = {x0[0]-x1[0],x0[1]-x1[1],x0[2]-x1[2]};
      double l2 = dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2];
      emin = fmin(l2,emin);
    }
  }
  return sqrt(emin);
}

static void fluid_problem_smooth_velocity(FluidProblem *problem, double lsmooth, double *smooth_velocity, double *smooth_c) {
  Mesh *mesh = problem->mesh;
  double emin = mesh_get_min_edge_length(mesh);
  int n = ceil(10*lsmooth*lsmooth/(emin*emin));
  double alpha = lsmooth*lsmooth/n;
  double *rhs = malloc(mesh->n_nodes*3*mesh->n_nodes);
  double *mass = malloc(mesh->n_nodes*mesh->n_nodes);
  for (int i = 0; i < mesh->n_nodes; ++i){
    smooth_velocity[i*2+0] = problem->solution[i*3+0];
    smooth_velocity[i*2+1] = problem->solution[i*3+1];
    smooth_c[i] = problem->porosity[i];
  }
  for (int iter = 0; iter < n; ++iter) {
    for (int i = 0; i < mesh->n_nodes; ++i){
      rhs[i*3+0] = 0;
      rhs[i*3+1] = 0;
      rhs[i*3+2] = 0;
      mass[i] = 0;
    }
    for (int iel=0; iel < mesh->n_triangles; ++iel) {
      const int *tri = &mesh->triangles[iel*3];
      const double x[3][2] = {
        {mesh->x[tri[0]*3+0],mesh->x[tri[0]*3+1]},
        {mesh->x[tri[1]*3+0],mesh->x[tri[1]*3+1]},
        {mesh->x[tri[2]*3+0],mesh->x[tri[2]*3+1]}};
      const double dxdxi[2][2] = {
        {x[1][0]-x[0][0],x[2][0]-x[0][0]},
        {x[1][1]-x[0][1],x[2][1]-x[0][1]}};
      const double detj = dxdxi[0][0]*dxdxi[1][1]-dxdxi[0][1]*dxdxi[1][0];
      const double dxidx[2][2] = {
        {dxdxi[1][1]/detj,-dxdxi[0][1]/detj},
        {-dxdxi[1][0]/detj,dxdxi[0][0]/detj}
      };
      const double dphi[][2] = {
        {-dxidx[0][0]-dxidx[1][0],-dxidx[0][1]-dxidx[1][1]},
        {dxidx[0][0],dxidx[0][1]},
        {dxidx[1][0],dxidx[1][1]}
      };
      double U[] = {smooth_velocity[tri[0]*2+0],smooth_velocity[tri[1]*2+0],smooth_velocity[tri[2]*2+0]};
      double V[] = {smooth_velocity[tri[0]*2+1],smooth_velocity[tri[1]*2+1],smooth_velocity[tri[2]*2+1]};
      double C[] = {smooth_c[tri[0]],smooth_c[tri[1]],smooth_c[tri[2]]};
      double du[2] = {
        dphi[0][0]*U[0]+dphi[1][0]*U[1]+dphi[2][0]*U[2],
        dphi[0][1]*U[0]+dphi[1][1]*U[1]+dphi[2][1]*U[2]
      };
      double dv[2] = {
        dphi[0][0]*V[0]+dphi[1][0]*V[1]+dphi[2][0]*V[2],
        dphi[0][1]*V[0]+dphi[1][1]*V[1]+dphi[2][1]*V[2]
      };
      double dc[2] = {
        dphi[0][0]*C[0]+dphi[1][0]*C[1]+dphi[2][0]*C[2],
        dphi[0][1]*C[0]+dphi[1][1]*C[1]+dphi[2][1]*C[2]
      };
      for (int i = 0; i < 3; ++i) {
        rhs[tri[i]*3+0] -= (dphi[i][0]*du[0] + dphi[i][1]*du[1])*detj/2;
        rhs[tri[i]*3+1] -= (dphi[i][0]*dv[0] + dphi[i][1]*dv[1])*detj/2;
        rhs[tri[i]*3+2] -= 0*(dphi[i][0]*dc[0] + dphi[i][1]*dc[1])*detj/2;
        mass[tri[i]] += detj/6;
      }
    }
    for (int i = 0; i < mesh->n_nodes; ++i) {
      smooth_velocity[i*2+0] += rhs[i*3+0]/mass[i]*alpha;
      smooth_velocity[i*2+1] += rhs[i*3+1]/mass[i]*alpha;
      smooth_c[i] += rhs[i*3+2]/mass[i]*alpha;
    }
  }
  free(mass);
  free(rhs);
}

void fluid_problem_compute_node_particle_force(FluidProblem *problem, double dt, double lsmooth, double *particle_force) {
  double *porosity = problem->porosity;
  Mesh *mesh = problem->mesh;
  double *smoothv = malloc(mesh->n_nodes*2*sizeof(double));
  double *smoothc = malloc(mesh->n_nodes*sizeof(double));
  fluid_problem_smooth_velocity(problem, lsmooth, smoothv,smoothc);
  const double *solution = problem->solution;
  for (int i = 0; i < mesh->n_nodes*2; ++i) {
    problem->node_force[i] = 0.0;
  }
  for (int i = 0; i < problem->n_particles; ++i) {
    double *xi = problem->particle_uvw + 2*i;
    double phi[3] = {1-xi[0]-xi[1],xi[0],xi[1]};
    int iel = problem->particle_element_id[i];
    double vol = problem->particle_volume[i];
    double mass = problem->particle_mass[i];
    particle_force[i*2+0] = 0;
    particle_force[i*2+1] = problem->g*(mass-problem->rho*vol);
    if(iel < 0){
      continue;
    }
    int *tri = problem->mesh->triangles+iel*3;
    double C[] = {smoothc[tri[0]],smoothc[tri[1]],smoothc[tri[2]]};
    double U[] = {smoothv[tri[0]*2+0],smoothv[tri[1]*2+0],smoothv[tri[2]*2+0]};
    double V[] = {smoothv[tri[0]*2+1],smoothv[tri[1]*2+1],smoothv[tri[2]*2+1]};
    double P[] = {solution[tri[0]*3+2],solution[tri[1]*3+2],solution[tri[2]*3+2]};
    double vf[2] = {
      phi[0]*U[0]+phi[1]*U[1]+phi[2]*U[2],
      phi[0]*V[0]+phi[1]*V[1]+phi[2]*V[2]
    };
    double c = phi[0]*C[0]+phi[1]*C[1]+phi[2]*C[2];
    const double x[3][2] = {
      {mesh->x[tri[0]*3+0],mesh->x[tri[0]*3+1]},
      {mesh->x[tri[1]*3+0],mesh->x[tri[1]*3+1]},
      {mesh->x[tri[2]*3+0],mesh->x[tri[2]*3+1]}};
    const double dxdxi[2][2] = {
      {x[1][0]-x[0][0],x[2][0]-x[0][0]},
      {x[1][1]-x[0][1],x[2][1]-x[0][1]}};
    const double detj = dxdxi[0][0]*dxdxi[1][1]-dxdxi[0][1]*dxdxi[1][0];
    const double dxidx[2][2] = {
      {dxdxi[1][1]/detj,-dxdxi[0][1]/detj},
      {-dxdxi[1][0]/detj,dxdxi[0][0]/detj}
    };
    const double dphi[][2] = {
      {-dxidx[0][0]-dxidx[1][0],-dxidx[0][1]-dxidx[1][1]},
      {dxidx[0][0],dxidx[0][1]},
      {dxidx[1][0],dxidx[1][1]}
    };
    double gradp[2] = {
      dphi[0][0]*P[0]+dphi[1][0]*P[1]+dphi[2][0]*P[2],
      dphi[0][1]*P[0]+dphi[1][1]*P[1]+dphi[2][1]*P[2]
    };
    double du[2] = {
      problem->particle_velocity[i*2+0]-vf[0]/c,
      problem->particle_velocity[i*2+1]-vf[1]/c
    };
    double rhop = mass/vol;
    double d = 2*sqrt(vol/M_PI);
    double drag[2];
    particle_drag(du,problem->mu,problem->rho,d,c,drag, dt, mass, rhop, gradp);
    /*FILE *log = fopen("DragForce.txt", "at");
    if (!log) log = fopen("DragForce.txt", "wt");
    fprintf(log, "%d, %e, %e, %e, %e, %e, %e\n", i, du[0], du[1], drag[0], drag[1], mass, vol);
    fclose(log);*/

    particle_force[i*2+0] += -drag[0]-vol*gradp[0];
    particle_force[i*2+1] += -drag[1]-vol*gradp[1];
    //printf("%e,%e,%e,%e,%e,%e\n",particle_force[i*2+0],particle_force[i*2+1],-drag[0],-drag[1],0.,problem->g*(mass-problem->rho*vol));
    for (int iphi = 0; iphi < 3; ++iphi) {
      problem->node_force[tri[iphi]*2+0] += drag[0]*phi[iphi];
      problem->node_force[tri[iphi]*2+1] += drag[1]*phi[iphi];
    }
  }
  for (int i = 0; i < mesh->n_nodes; ++i) {
    problem->node_force[i*2+0] /= problem->node_volume[i];
    problem->node_force[i*2+1] /= problem->node_volume[i];
  }
  free(smoothv);
}

static void fluid_problem_assemble_system(FluidProblem *problem, const double *solution_old, double dt)
{
  LinearSystem *lsys = problem->linear_system;
  const Mesh *mesh = problem->mesh;
  const double *solution = problem->solution;
  const double *porosity = problem->porosity;
  const double *old_porosity = problem->old_porosity;
  const double *node_force = problem->node_force;
  double *local_vector = malloc(sizeof(double)*n_fields*3);
  double *local_matrix = malloc(sizeof(double)*n_fields*n_fields*9);
  const double mu = problem->mu;
  const double rho = problem->rho;
  const double *epsilon = problem->epsilon;
  for (int iel=0; iel < mesh->n_triangles; ++iel) {
    const int *tri = &mesh->triangles[iel*3];
    const double x[3][2] = {
      {mesh->x[tri[0]*3+0],mesh->x[tri[0]*3+1]},
      {mesh->x[tri[1]*3+0],mesh->x[tri[1]*3+1]},
      {mesh->x[tri[2]*3+0],mesh->x[tri[2]*3+1]}};
    const double dxdxi[2][2] = {
      {x[1][0]-x[0][0],x[2][0]-x[0][0]},
      {x[1][1]-x[0][1],x[2][1]-x[0][1]}};
    const double detj = dxdxi[0][0]*dxdxi[1][1]-dxdxi[0][1]*dxdxi[1][0];
    const double dxidx[2][2] = {
      {dxdxi[1][1]/detj,-dxdxi[0][1]/detj},
      {-dxdxi[1][0]/detj,dxdxi[0][0]/detj}
    };
    const double dphi[][2] = {
      {-dxidx[0][0]-dxidx[1][0],-dxidx[0][1]-dxidx[1][1]},
      {dxidx[0][0],dxidx[0][1]},
      {dxidx[1][0],dxidx[1][1]}
    };
    for (int i=0; i < 3*n_fields; ++i)
      local_vector[i] = 0;
    for (int i=0; i < 9*n_fields*n_fields; ++i)
      local_matrix[i] = 0;
    double C[] = {porosity[tri[0]],porosity[tri[1]],porosity[tri[2]]};
    double OLDC[] = {old_porosity[tri[0]],old_porosity[tri[1]],old_porosity[tri[2]]};
    double DCDT[] = {(C[0]-OLDC[0])/dt,(C[1]-OLDC[1])/dt,(C[2]-OLDC[2])/dt};
    double U[] = {solution[tri[0]*3+0],solution[tri[1]*3+0],solution[tri[2]*3+0]};
    double V[] = {solution[tri[0]*3+1],solution[tri[1]*3+1],solution[tri[2]*3+1]};
    double FU[] = {node_force[tri[0]*2+0], node_force[tri[1]*2+0], node_force[tri[2]*2+0]};
    double FV[] = {node_force[tri[0]*2+1], node_force[tri[1]*2+1], node_force[tri[2]*2+1]};
    double U_old[] = {solution_old[tri[0]*3+0],solution_old[tri[1]*3+0],solution_old[tri[2]*3+0]};
    double V_old[] = {solution_old[tri[0]*3+1],solution_old[tri[1]*3+1],solution_old[tri[2]*3+1]};
    double P[] = {solution[tri[0]*3+2],solution[tri[1]*3+2],solution[tri[2]*3+2]};
    double dc[2] = {
      dphi[0][0]*C[0]+dphi[1][0]*C[1]+dphi[2][0]*C[2],
      dphi[0][1]*C[0]+dphi[1][1]*C[1]+dphi[2][1]*C[2]
    };
    double du[2] = {
      dphi[0][0]*U[0]+dphi[1][0]*U[1]+dphi[2][0]*U[2],
      dphi[0][1]*U[0]+dphi[1][1]*U[1]+dphi[2][1]*U[2]
    };
    double dv[2] = {
      dphi[0][0]*V[0]+dphi[1][0]*V[1]+dphi[2][0]*V[2],
      dphi[0][1]*V[0]+dphi[1][1]*V[1]+dphi[2][1]*V[2]
    };
    double dp[2] = {
      dphi[0][0]*P[0]+dphi[1][0]*P[1]+dphi[2][0]*P[2],
      dphi[0][1]*P[0]+dphi[1][1]*P[1]+dphi[2][1]*P[2]
    };
    for (int iqp = 0; iqp< N_QP; ++iqp) {
      const double xi = QP[iqp][0], eta = QP[iqp][1];
      const double phi[] = {1-xi-eta, xi, eta};
      double u = phi[0]*U[0]+phi[1]*U[1]+phi[2]*U[2];
      double v = phi[0]*V[0]+phi[1]*V[1]+phi[2]*V[2];
      double fu = phi[0]*FU[0]+phi[1]*FU[1]+phi[2]*FU[2];
      double fv = phi[0]*FV[0]+phi[1]*FV[1]+phi[2]*FV[2];
      double u_old = phi[0]*U_old[0]+phi[1]*U_old[1]+phi[2]*U_old[2];
      double v_old = phi[0]*V_old[0]+phi[1]*V_old[1]+phi[2]*V_old[2];
      double p = phi[0]*P[0]+phi[1]*P[1]+phi[2]*P[2];
      double c = phi[0]*C[0]+phi[1]*C[1]+phi[2]*C[2];
      double dcdt = phi[0]*DCDT[0]+phi[1]*DCDT[1]+phi[2]*DCDT[2];
      const double jw = QW[iqp]*detj;
      for (int iphi = 0; iphi < 3; ++iphi) {
        double phii = phi[iphi];
        double dphii[2] = {dphi[iphi][0],dphi[iphi][1]};
        double tau[2][2] = {{du[0]-u*dc[0]/c,du[1]-u*dc[1]/c},
                            {dv[0]-v*dc[0]/c,dv[1]-v*dc[1]/c}};
        local_vector[iphi+0] += jw*(
           2*mu*(dphii[0]*tau[0][0]+dphii[1]*(tau[0][1]+tau[1][0])*0.5)
          +rho*phii*((u-u_old)/dt-u/c *dcdt+(u*tau[0][0]+v*tau[0][1])/c)
          -p*dphii[0]-fu*phii
        );
        local_vector[iphi+3] += jw*(
           2*mu*(dphii[0]*(tau[0][1]+tau[1][0])*0.5+dphii[1]*tau[1][1])
          +rho*phii*((v-v_old)/dt-v/c *dcdt+(u*tau[1][0]+v*tau[1][1])/c)
          -p*dphii[1]-fv*phii
        );

        local_vector[iphi+6] += jw*(
           epsilon[iel]*(dphii[0]*dp[0]+dphii[1]*dp[1])
           +phii*((du[0]+dv[1])+dcdt)
        );
        for (int jphi = 0; jphi < 3; ++jphi) {
          double phij = phi[jphi];
          double dphij[2] = {dphi[jphi][0],dphi[jphi][1]};
          double dtau[2] = {dphij[0]-phij*dc[0]/c,dphij[1]-phij*dc[1]/c};
          local_matrix[(iphi+0)*9+jphi+0] += jw*2*mu*(dphii[0]*dtau[0]+dphii[1]*dtau[1]*0.5)  // U U
                                            +jw*rho*phii*(phij/dt-phij/c*dcdt+(phij*tau[0][0]+u*dtau[0]+v*dtau[1])/c);
          local_matrix[(iphi+0)*9+jphi+3] += jw*2*mu*dphii[1]*dtau[0]*0.5                     // U V
                                            +jw*rho*phii*phij*tau[0][1]/c;
          local_matrix[(iphi+0)*9+jphi+6] +=-jw*dphii[0]*phij;                                // U P
          local_matrix[(iphi+3)*9+jphi+0] += jw*2*mu*dphii[0]*dtau[1]*0.5                     // V U
                                            +jw*rho*phii*phij*tau[1][0]/c;
          local_matrix[(iphi+3)*9+jphi+3] += jw*2*mu*(dphii[0]*dtau[0]*0.5+dphii[1]*dtau[1])  // V V
                                            +jw*rho*phii*(phij/dt-phij/c*dcdt+(phij*tau[1][1]+u*dtau[0]+v*dtau[1])/c);
          local_matrix[(iphi+3)*9+jphi+6] +=-jw*dphii[1]*phij;                                // V P
          local_matrix[(iphi+6)*9+jphi+0] += jw*phii*dphij[0];                                // P U
          local_matrix[(iphi+6)*9+jphi+3] += jw*phii*dphij[1];                                // P V
          local_matrix[(iphi+6)*9+jphi+6] += jw*epsilon[iel]*(dphii[0]*dphij[0]+dphii[1]*dphij[1]);// P P
        }
      }
    }
    linear_system_add_to_matrix(lsys,iel,iel,local_matrix);
    linear_system_add_to_rhs(lsys,iel,local_vector);
  }
  free(local_vector);
  free(local_matrix);

  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){
      linear_system_fix_row(lsys, mesh->phys_nodes[iphys][i], bnd->field, 0.);
    }
  }
}

static void apply_boundary_conditions(const Mesh *mesh, 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)*2);
    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];
      x[i*2+0] = mesh->x[j*3+0];
      x[i*2+1] = mesh->x[j*3+1];
    }
    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);
  }
}

void 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->solution, problem->n_strong_boundaries, problem->strong_boundaries);
  double *solution_old = 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];
  double *dx = malloc(sizeof(double)*mesh->n_nodes*n_fields);
  double norm0 = 0;
  for (int i=0; i<newton_max_it; ++i) {
    linear_system_zero_matrix_and_rhs(problem->linear_system);
    fluid_problem_assemble_system(problem, solution_old, dt);
    double norm = linear_system_get_rhs_norm(problem->linear_system);
    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);
    linear_system_solve(problem->linear_system, 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];
    }
  }
  printf("total solve time : %g\n", totaltime);
  free(dx);
  free(solution_old);
}

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_triangles; ++iel) {
    const int *tri = &mesh->triangles[iel*3];
    const double x[3][2] = {
      {mesh->x[tri[0]*3+0],mesh->x[tri[0]*3+1]},
      {mesh->x[tri[1]*3+0],mesh->x[tri[1]*3+1]},
      {mesh->x[tri[2]*3+0],mesh->x[tri[2]*3+1]}};
    const double dxdxi[2][2] = {
      {x[1][0]-x[0][0],x[2][0]-x[0][0]},
      {x[1][1]-x[0][1],x[2][1]-x[0][1]}};
    const double detj = dxdxi[0][0]*dxdxi[1][1]-dxdxi[0][1]*dxdxi[1][0];
    problem->node_volume[tri[0]] += detj/6;
    problem->node_volume[tri[1]] += detj/6;
    problem->node_volume[tri[2]] += detj/6;
  }
}

static void computeEpsilon(FluidProblem *problem, bool autoEpsilon)
{
  double alpha = problem->alpha;
  double nu = problem->mu/problem->rho;
  const Mesh *mesh = problem->mesh;
  double a,b,c,h;
  double epsMax = 0;
  double epsMin = 10000;
  for(int iel = 0; iel < mesh->n_triangles; ++iel)
  {
    if (autoEpsilon)
    {
      const int *tri = &mesh->triangles[iel*3];
      const double x[3][2] = {
        {mesh->x[tri[0]*3+0],mesh->x[tri[0]*3+1]},
        {mesh->x[tri[1]*3+0],mesh->x[tri[1]*3+1]},
        {mesh->x[tri[2]*3+0],mesh->x[tri[2]*3+1]}};
      a = sqrt(pow((x[0][0]-x[1][0]),2)+pow((x[0][1]-x[1][1]),2));
      b = sqrt(pow((x[1][0]-x[2][0]),2)+pow((x[1][1]-x[2][1]),2));
      c = sqrt(pow((x[2][0]-x[0][0]),2)+pow((x[2][1]-x[0][1]),2));
      h = a*b*c/sqrt((a+b+c)*(b+c-a)*(c+a-b)*(a+b-c));
      problem->epsilon[iel] = alpha*h*h/nu;
      if (problem->epsilon[iel] > epsMax)
      {
        epsMax = problem->epsilon[iel];
      }
      if (problem->epsilon[iel] < epsMin)
      {
        epsMin = problem->epsilon[iel];
      }
    }
    else
    {
      problem->epsilon[iel] = alpha;
    }
  }
  printf("Epsilon Max = %g\n",epsMax);
  printf("Epsilon Min = %g\n",epsMin);
}

FluidProblem *fluid_problem_new(const char *mesh_file_name, double g, double mu, double rho, double alpha, bool autoEpsilon, int n_strong_boundaries, StrongBoundary *strong_boundaries)
{
  FluidProblem *problem = malloc(sizeof(FluidProblem));
  problem->rho = rho;
  problem->alpha = alpha;
  problem->mu = mu;
  problem->g = g;
  Mesh *mesh = mesh_load_msh(mesh_file_name);
  problem->epsilon = malloc(sizeof(double)*mesh->n_triangles);
  if (!mesh)
    return NULL;
  problem->mesh = mesh;
  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->porosity = malloc(mesh->n_nodes*sizeof(double));
  problem->node_force = malloc(mesh->n_nodes*2*sizeof(double));
  for (int i = 0; i < mesh->n_nodes*2; ++i) {
    problem->node_force[i] = 0.0;
  }
  problem->old_porosity = malloc(mesh->n_nodes*sizeof(double));
  problem->solution = malloc(mesh->n_nodes*n_fields*sizeof(double));
  // begin to remove
  //static StrongBoundary strong_boundaries[] = {{"Bottom",1,VEXT},{"Bottom",0,0.},{"Top",0,0.},{"PtFix",2,0.},{"Box",0,0.},{NULL, 0, 0.}};
  //static StrongBoundary strong_boundaries[] = {{"Top",2,0},{"Top",1,0.},{"Top",0,0.},{"Box",1,0.},{"Box",0,0.},{NULL, 0, 0.}};
  for (int i = 0; i < problem->mesh->n_nodes; ++i){
    problem->solution[i*3+0] = 0.;
    problem->solution[i*3+1] = 0;
    problem->solution[i*3+2] = 0.;
  }
  problem->node_volume = malloc(0);
  fluid_problem_compute_node_volume(problem);
  // end to remove
  problem->linear_system = linear_system_new(mesh->n_triangles, 3, n_fields, mesh->triangles); 
  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_volume = malloc(0);
  problem->particle_velocity = malloc(0);
  computeEpsilon(problem, autoEpsilon);
  return problem;
}

void fluid_problem_free(FluidProblem *problem)
{
  free(problem->solution);
  free(problem->node_force);
  free(problem->porosity);
  free(problem->old_porosity);
  free(problem->epsilon);
  linear_system_free(problem->linear_system);
  free(problem->particle_uvw);
  free(problem->particle_element_id);
  free(problem->particle_position);
  free(problem->particle_mass);
  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_free(problem->mesh);
}

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->porosity[i] = 0;
  }
  for (int i = 0; i < problem->n_particles; ++i) {
    if(problem->particle_element_id[i] == -1) continue;
    const int *tri = &mesh->triangles[problem->particle_element_id[i]*3];
    double u = problem->particle_uvw[i*2+0], v = problem->particle_uvw[i*2+1];
    problem->porosity[tri[0]] += (1-u-v)*volume[i];
    problem->porosity[tri[1]] += u*volume[i];
    problem->porosity[tri[2]] += v*volume[i];
  }
  for (int i = 0; i < mesh->n_nodes; ++i){
    problem->porosity[i] = (1-problem->porosity[i]/problem->node_volume[i]);
  }
}

void fluid_problem_adapt_mesh(FluidProblem *problem, double gradmin, double gradmax, double lcmin, bool autoEpsilon)
{
  Mesh *mesh = problem->mesh;
  double *solution = problem->solution;
  double *new_mesh_size = malloc(sizeof(double)*mesh->n_nodes);
  for (int i = 0; i < mesh->n_nodes; ++i)
    new_mesh_size[i] = DBL_MAX;
  gradmax = sqrt(gradmax);
  gradmin = sqrt(gradmin);
  double *porosity = problem->porosity;
  for (int iel = 0; iel < mesh->n_triangles; ++iel) {
    int *tri = problem->mesh->triangles+iel*3;
    double C[] = {porosity[tri[0]], porosity[tri[1]], porosity[tri[2]]};
    double U[] = {solution[tri[0]*3+0],solution[tri[1]*3+0],solution[tri[2]*3+0]};
    double V[] = {solution[tri[0]*3+1],solution[tri[1]*3+1],solution[tri[2]*3+1]};
    const double x[3][2] = {
      {mesh->x[tri[0]*3+0],mesh->x[tri[0]*3+1]},
      {mesh->x[tri[1]*3+0],mesh->x[tri[1]*3+1]},
      {mesh->x[tri[2]*3+0],mesh->x[tri[2]*3+1]}};
    const double dxdxi[2][2] = {
      {x[1][0]-x[0][0],x[2][0]-x[0][0]},
      {x[1][1]-x[0][1],x[2][1]-x[0][1]}};
    const double detj = dxdxi[0][0]*dxdxi[1][1]-dxdxi[0][1]*dxdxi[1][0];
    const double dxidx[2][2] = {
      {dxdxi[1][1]/detj,-dxdxi[0][1]/detj},
      {-dxdxi[1][0]/detj,dxdxi[0][0]/detj}
    };
    const double dphi[][2] = {
      {-dxidx[0][0]-dxidx[1][0],-dxidx[0][1]-dxidx[1][1]},
      {dxidx[0][0],dxidx[0][1]},
      {dxidx[1][0],dxidx[1][1]}
    };
    double gradU[2] = {
      dphi[0][0]*U[0]/C[0]+dphi[1][0]*U[1]/C[1]+dphi[2][0]*U[2]/C[2],
      dphi[0][1]*U[0]/C[0]+dphi[1][1]*U[1]/C[1]+dphi[2][1]*U[2]/C[2]
    };
    double gradV[2] = {
      dphi[0][0]*V[0]/C[0]+dphi[1][0]*V[1]/C[1]+dphi[2][0]*V[2]/C[2],
      dphi[0][1]*V[0]/C[0]+dphi[1][1]*V[1]/C[1]+dphi[2][1]*V[2]/C[2]
    };
    double ngrad = pow(gradU[0]*gradU[0]+gradU[1]*gradU[1] + gradV[0]*gradV[0] + gradV[1]*gradV[1], 0.25);
    double lc = lcmin /fmin(1,fmax(ngrad/gradmax,gradmin/gradmax));
    for (int j = 0; j < 3; ++j)
      new_mesh_size[tri[j]] = fmin(new_mesh_size[tri[j]], lc);
  }
  FILE *f = fopen("adapt/lc.pos", "w");
  if(!f)
    printf("cannot open file adapt/lc.pos for writing\n");
  else
    printf("open ok\n");
  fprintf(f, "View \"lc\" {\n");
    printf("write ok\n");
  for (int iel = 0; iel < mesh->n_triangles; ++iel) {
    int *tri = problem->mesh->triangles+iel*3;
    const double x[3][2] = {
      {mesh->x[tri[0]*3+0],mesh->x[tri[0]*3+1]},
      {mesh->x[tri[1]*3+0],mesh->x[tri[1]*3+1]},
      {mesh->x[tri[2]*3+0],mesh->x[tri[2]*3+1]}};
    double LC[3] = {new_mesh_size[tri[0]], new_mesh_size[tri[1]], new_mesh_size[tri[2]]};
    fprintf(f, "ST(%.8g, %.8g, 0, %.8g, %.8g, 0, %.8g, %.8g, 0){%.8g, %.8g, %.8g};\n",
        x[0][0],x[0][1],x[1][0],x[1][1],x[2][0],x[2][1],
        LC[0],LC[1],LC[2]);
  }
  fprintf(f, "};\n");
  fclose(f);
  free(new_mesh_size);
  system("gmsh -2 adapt/mesh.geo");

  Mesh *new_mesh = mesh_load_msh("adapt/mesh.msh");
  double *new_solution = malloc(sizeof(double)*new_mesh->n_nodes*3);
  double *new_xi = malloc(sizeof(double)*new_mesh->n_nodes*2);
  int *new_eid = malloc(sizeof(int)*new_mesh->n_nodes);
  double *newx = malloc(sizeof(double)*2*new_mesh->n_nodes);
  for (int i = 0;i < new_mesh->n_nodes;++i) {
    newx[i*2+0] = new_mesh->x[i*3+0];
    newx[i*2+1] = new_mesh->x[i*3+1];
  }
  mesh_particles_to_mesh(mesh, new_mesh->n_nodes, newx, new_eid, new_xi);
  for (int i = 0; i < new_mesh->n_nodes; ++i) {
    int *tri = problem->mesh->triangles+new_eid[i]*3;
    if(new_eid[i] < 0)
      continue;
    double U[] = {solution[tri[0]*3+0],solution[tri[1]*3+0],solution[tri[2]*3+0]};
    double V[] = {solution[tri[0]*3+1],solution[tri[1]*3+1],solution[tri[2]*3+1]};
    double *xi = new_xi+i*2;
    double phi[] = {1-xi[0]-xi[1],xi[0],xi[1]};
    new_solution[i*3+0] = U[0]*phi[0]+U[1]*phi[1]+U[2]*phi[2];
    new_solution[i*3+1] = V[0]*phi[0]+V[1]*phi[1]+V[2]*phi[2];
    new_solution[i*3+2] = 0.;
  }
  free(new_eid);
  free(new_xi);
  free(newx);
  free(problem->solution);
  problem->solution = new_solution;
  free(problem->porosity);
  problem->porosity = malloc(sizeof(double)*new_mesh->n_nodes);
  free(problem->old_porosity);
  problem->old_porosity = malloc(sizeof(double)*new_mesh->n_nodes);
  free(problem->node_force);
  problem->node_force = malloc(sizeof(double)*2*new_mesh->n_nodes);
  mesh_free(problem->mesh);
  problem->mesh = new_mesh;
  free(problem->epsilon);
  problem->epsilon = malloc(sizeof(double)*new_mesh->n_triangles);
  mesh_particles_to_mesh(problem->mesh, problem->n_particles, problem->particle_position, problem->particle_element_id, problem->particle_uvw);
  fluid_problem_compute_node_volume(problem);
  fluid_problem_compute_porosity(problem);
  computeEpsilon(problem, autoEpsilon);
  linear_system_free(problem->linear_system);
  problem->linear_system = linear_system_new(new_mesh->n_triangles, 3, n_fields, new_mesh->triangles); 
}

void fluid_problem_set_particles(FluidProblem *problem, int n, double *mass, double *volume, double *position, double *velocity)
{
  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_volume);
    free(problem->particle_velocity);
    problem->particle_uvw = malloc(sizeof(double)*2*n);
    problem->particle_element_id = malloc(sizeof(int)*n);
    problem->particle_position = malloc(sizeof(double)*2*n);
    problem->particle_mass = malloc(sizeof(double)*n);
    problem->particle_volume = malloc(sizeof(double)*n);
    problem->particle_velocity = malloc(sizeof(double)*n*2);
  }
  mesh_particles_to_mesh(problem->mesh, n, position, problem->particle_element_id, problem->particle_uvw);
  for (int i = 0; i < n; ++i) {
    problem->particle_position[i*2+0] = position[i*2+0];
    problem->particle_position[i*2+1] = position[i*2+1];
    problem->particle_mass[i] = mass[i];
    problem->particle_volume[i] = volume[i];
    problem->particle_velocity[i*2+0] = velocity[i*2+0];
    problem->particle_velocity[i*2+1] = velocity[i*2+1];
  }
  for (int i = 0; i < problem->mesh->n_nodes; ++i){
    problem->old_porosity[i] = problem->porosity[i];
  }
  fluid_problem_compute_porosity(problem);
}