/*******************************************************************************
 * ALBERTA:  an Adaptive multi Level finite element toolbox using
 *           Bisectioning refinement and Error control by Residual
 *           Techniques for scientific Applications
 *
 * file:     parametric.c
 *
 *
 * description: Support for parametric elements.
 *
 *******************************************************************************
 *
 *  authors:   Daniel Koester
 *             Institut fuer Mathematik
 *             Universitaet Augsburg
 *             Universitaetsstr. 14
 *             D-86159 Augsburg, Germany
 *
 *             Claus-Justus Heine
 *             Numerik fuer Hoechstleistungsrechner
 *             Universität Stuttgart
 *             Pfaffenwaldring 57
 *             70569 Stuttgart, Germany
 *
 *  http://www.alberta-fem.de
 *
 *  (c) by D. Koester (2005),
 *         C.-J. Heine (2006-2012).
 *
 ******************************************************************************/

#include "alberta_intern.h"
#include "alberta.h"

#define LAGRANGE_MAGIC { 'L', 'P', 'A', 'R' }
#define MAGIC_STRING   "LPAR"

#define DEGREE_GEN 3 /* minimal polynomial degree with generic implementation */
#define DEGREE_MAX 4 /* maximal polynomial degree supported */

typedef struct lagrange_param_data LAGRANGE_PARAM_DATA;
struct lagrange_param_data
{
  char            magic[4];
  int             degree;
  PARAM_STRATEGY  strategy;
  NODE_PROJECTION *n_proj;
  DOF_REAL_D_VEC  *coords;

  DOF_PTR_VEC     *edge_projections; /* The projections for each edge
                                      * or NULL if this edge is not
                                      * subject to a projection.
                                      */

  REAL_D          *local_coords;  /* points to either
				   * LAGRANGE_PARAM_DATA::param_local_coords
				   * or to the current EL_INFO::coord
				   * space if the current element is
				   * affine and after
				   * PARAMETRIC::init_element() has
				   * been called.
				   */

  REAL_D          *param_local_coords; /* allocated space for the
					* local coordinate vector,
					* filled by
					* PARAMETRIC::init_element().
					*/

  int             n_local_coords;  /* == n_bas_fcts */
  int             i_am_affine;     /* cached return value of
				    * PARAMETRIC::init_element()
				    * (actually its negation).
				    */

  EL              *el; /* pointer to the element from the last
			* PARAMETRIC::init_element() invocation.
			*/
  
  int  max_iter;		/* maximum number of iterations and */
  REAL newton_tolerance;	/* tolerance for newton's method in */
				/* param_world_to_coord */
  REAL lambda_tolerance;	/* specify when lambda[k] is considered < 0 */
};

static void param_coord_to_world(const EL_INFO *el_info, const QUAD *quad, 
				 int N, const REAL_B lambda[],
				 REAL_D *world);

static void param_world_to_coord(const EL_INFO *el_info, int N,
				 const REAL_D world[],
				 REAL_B lambda[], int k[]);

/* Interface to ALBERTA's Newton iterators. */
static void nl_update(void *data,
		      int dim, const REAL *lambda_k, bool upd_DF, REAL *Fx);
static int nl_solve(void *data, int dim, const REAL *rhs, REAL *d);
typedef struct isp_data 
{
  REAL_D            x;		/* world */
  REAL_D           *coords;	/* coords of the quad points */
  REAL              Df[DIM_OF_WORLD+1][DIM_OF_WORLD+1];
  BAS_FCTS         *bfcts;
  const EL_INFO    *el_info;
} ISP_DATA;

#include "../0d/parametric_0d.c"
#if DIM_MAX > 0
#include "../1d/parametric_1d.c"
#if DIM_MAX > 1
#include "../2d/parametric_2d.c"
#if DIM_MAX > 2
#include "../3d/parametric_3d.c"
#endif
#endif
#endif

static const PARAMETRIC *all_lagrange[DIM_MAX+1][DEGREE_GEN] = {
  { &lagrange_parametric_0d,
    &lagrange_parametric_0d,
    &lagrange_parametric_0d },
  { &lagrange_parametric1_1d,
    &lagrange_parametric2_1d,
    &lagrange_parametricY_1d }
#if DIM_MAX > 1
  ,
  { &lagrange_parametric1_2d,
    &lagrange_parametric2_2d,
    &lagrange_parametricY_2d }
#if DIM_MAX > 2
  ,
  { &lagrange_parametric1_3d,
    &lagrange_parametric2_3d,
    &lagrange_parametricY_3d }
#endif
#endif
};

static void (*all_refine_interpol[DIM_MAX+1][DEGREE_GEN])
  (DOF_REAL_D_VEC *, RC_LIST_EL *, int) = {
  { NULL,
    NULL,
    NULL },
  { refine_interpol1_1d,
    refine_interpol2_1d,
    refine_interpolY_1d }
#if DIM_MAX > 1
  ,
  { refine_interpol1_2d,
    refine_interpol2_2d,
    refine_interpolY_2d }
#if DIM_MAX > 2
  ,
  { refine_interpol1_3d,
    refine_interpol2_3d,
    refine_interpolY_3d }
#endif
#endif
};

static void (*all_coarse_interpol[DIM_MAX+1][DEGREE_GEN])
     (DOF_REAL_D_VEC *, RC_LIST_EL *, int) = {
  { NULL,
    NULL,
    NULL },
  { NULL,
    coarse_interpol2_1d,
    coarse_interpolY_1d }
#if DIM_MAX > 1
  ,
  { NULL,
    coarse_interpol2_2d,
    coarse_interpolY_2d }
#if DIM_MAX > 2
  ,
  { NULL,
    coarse_interpol2_3d,
    coarse_interpolY_3d }
#endif
#endif
};

static void (*all_fill_coords[DIM_MAX+1][DEGREE_GEN])
  (LAGRANGE_PARAM_DATA *data) = {
  { fill_coords_0d,
    fill_coords_0d,
    fill_coords_0d },
  { fill_coords1_1d,
    fill_coords2_1d,
    fill_coordsY_1d }
#if DIM_MAX > 1
  ,
  { fill_coords1_2d,
    fill_coords2_2d,
    fill_coordsY_2d }
#if DIM_MAX > 2
  ,
  { fill_coords1_3d,
    fill_coords2_3d,
    fill_coordsY_3d }
#endif
#endif
};

static void (*alloc_quad_metadata[DIM_MAX+1])(const QUAD *quad,
					      const BAS_FCTS *bas_fcts) = {
  NULL,
  alloc_param_quad_metadata_1d,
#if DIM_OF_WORLD > 1
  alloc_param_quad_metadata_2d,
# if DIM_OF_WORLD > 2 
  alloc_param_quad_metadata_3d,
# endif
#endif
};

typedef struct param_quad_metadata
{
  const QUAD_FAST *quad_fast;
} PARAM_QUAD_METADATA;

static void param_coord_to_world(const EL_INFO *el_info, const QUAD *quad, 
				 int N, const REAL_B lambda[],
				 REAL_D *world)
{
  int i, iq, dim;
  LAGRANGE_PARAM_DATA *data = 
    (LAGRANGE_PARAM_DATA *)el_info->mesh->parametric->data;
  REAL_D *local_coords      = data->local_coords;
  const BAS_FCTS *bas_fcts  = data->coords->fe_space->bas_fcts;

  if (quad) {
    if (!data->i_am_affine) {
      QUAD_METADATA       *qmd = (QUAD_METADATA *)quad->metadata;
      PARAM_QUAD_METADATA *metadata;
      const QUAD_FAST     *quad_fast;
      const REAL          *const*phi;
      
      metadata = (PARAM_QUAD_METADATA *)qmd->param_data[bas_fcts->degree];
      if (metadata == NULL) {
	alloc_quad_metadata[bas_fcts->dim](quad, bas_fcts);
	metadata = (PARAM_QUAD_METADATA *)qmd->param_data[bas_fcts->degree];
      }

      quad_fast = metadata->quad_fast;

      (void)INIT_ELEMENT(el_info, quad_fast);

      phi = quad_fast->phi;

      for (iq = 0; iq < quad->n_points; iq++) {
	SET_DOW(0.0, world[iq]);

	for (i = 0; i < quad_fast->n_bas_fcts; i++) {
	  AXPY_DOW(phi[iq][i], local_coords[i], world[iq]);
	}
      }
    } else {

      (void)INIT_ELEMENT(el_info, quad);

      dim = quad->dim;

      for (iq = 0; iq < quad->n_points; iq++) {
	SET_DOW(0.0, world[iq]);

	for (i = 0; i < N_VERTICES(dim); i++) {
	  AXPY_DOW(quad->lambda[iq][i], local_coords[i], world[iq]);
	}
      }
    }
  } else {
    REAL phi_lambda;

    if (!data->i_am_affine) {
      for (iq = 0; iq < N; iq++) {
	SET_DOW(0.0, world[iq]);

	for (i = 0; i < bas_fcts->n_bas_fcts; i++) {
	  phi_lambda = PHI(bas_fcts, i, lambda[iq]);   

	  AXPY_DOW(phi_lambda, local_coords[i], world[iq]);
	}
      }
    } else {
      dim = el_info->mesh->dim;

      for (iq = 0; iq < N; iq++) {
	SET_DOW(0.0, world[iq]);

	for (i = 0; i < N_VERTICES(dim); i++) {
	  AXPY_DOW(lambda[iq][i], local_coords[i], world[iq]);
	}
      }
    }
  }

  return;
} 

/* return: -2 if newton did not converge,
 *         -1 if inside,
 *          otherwise index of lambda <0
 */
static void param_world_to_coord(const EL_INFO *el_info, int N,
				 const REAL_D world[],
				 REAL_B lambda[], int k[])
{
  FUNCNAME("param_world_to_coord");

/* stuff for newton */
  static ISP_DATA isp_data;
  static NLS_DATA nls_data = {
    nl_update,
    &isp_data,
    nl_solve,
    &isp_data,
    NULL,  /* norm */
    NULL,  /* norm-data */
    NULL,  /* work-space */
    1e-15, /* tolerance */
    1,     /* restart */
    1000,  /* max_iter */
    1,     /* info */
    -1.0,  /* initial residual */
    -1.0   /* residual */
  };
  REAL_B      lambda0_const, lambda0;
  int         iter, max_iter;
  REAL        tolerance, lambda_tolerance;
  int         dim_of_mesh = MIN(DIM_MAX, el_info->mesh->dim);
  int         fail_count  = 0;
  int         i, iq/* , dim */;
  int         k_dummy[N];
  
  LAGRANGE_PARAM_DATA *data = 
    (LAGRANGE_PARAM_DATA *)el_info->mesh->parametric->data;
  REAL_D *local_coords      = data->local_coords;
  const BAS_FCTS *bas_fcts  = data->coords->fe_space->bas_fcts;

  REAL_D coord_backup[N_VERTICES(dim_of_mesh)];

  nls_data.max_iter  = max_iter  = data->max_iter;
  nls_data.tolerance = tolerance = data->newton_tolerance;
  lambda_tolerance               = data->lambda_tolerance;

  if (!k) k = k_dummy;

  if (DIM_OF_WORLD != dim_of_mesh) {
    ERROR_EXIT("DIM_OF_WORLD = %d != %d = dim_of_mesh.",
	       DIM_OF_WORLD, dim_of_mesh);
  }
  DEBUG_TEST_EXIT((el_info->fill_flag & FILL_COORDS) ||
		  el_info->mesh->parametric->use_reference_mesh,
		  "You must enable the use_reference_mesh entry in the "
		  "PARAMETRIC structure to use this function.\n");

  if (!data->i_am_affine) {
    for (iq = 0; iq < N; iq++) {
      /* fill data */
      COPY_DOW(world[iq], isp_data.x);
      isp_data.el_info  = el_info;
      isp_data.bfcts    = (BAS_FCTS*)bas_fcts;
      isp_data.coords   = local_coords;

      /* For planar and not too heavily deformed meshes the standard
       * world_to_coord should provide a reasonable initial value for
       * points inside the simplex. If world lies outside the simplex
       * we can neither guarantee the convergence of newton's method
       * nor be sure that our initial lambda ist near the solution.
       */
      /* As global_refine does not fill the coords of projected nodes
       * into el_info_coord when use_reference_mesh == true (and not
       * at all if use_reference_mesh == false), we update el_info
       * with the correct information.
       */
      for (i = 0; i < N_VERTICES(dim_of_mesh); i++) {
	COPY_DOW(el_info->coord[i], coord_backup[i]);
	COPY_DOW(local_coords[i], (REAL *)el_info->coord[i]);
      }

      world_to_coord(el_info, world[iq], lambda0_const);

      /* Undo the hack above. */
      for (i=0; i < N_VERTICES(dim_of_mesh); i++) {
	COPY_DOW(coord_backup[i], (REAL *)el_info->coord[i]);
      }
      
      /* start newton, and retry if not successful */
      for (fail_count = 0; fail_count < N_LAMBDA(dim_of_mesh)+1;) {
	REAL sum = 0.0;
	
	COPY_BAR(DIM_MAX, lambda0_const, lambda0);

	if (fail_count > 0) {
	  #if 0
	  WARNING("Newton failed! current number of fails: %d.\n",
		  fail_count);
	  MSG("Retry with different lambda0...\n");
	  #endif
	  lambda0[fail_count-1] += 0.1;

	  for (i = 0; i < N_LAMBDA(dim_of_mesh); i++) sum += lambda0[i];
	  for (i = 0; i < N_LAMBDA(dim_of_mesh); i++) lambda0[i] /= sum;
	}

	iter = nls_newton(&nls_data, N_LAMBDA(dim_of_mesh), (REAL *)&lambda0);
	
	if (iter > max_iter) {
	  fail_count++;
	} else {
	  sum = 0;
	  for (i = 0; i < N_LAMBDA(dim_of_mesh); i++) {
	    lambda[iq][i] = lambda0[i];
	    sum += lambda0[i];
	  }
	  if (ABS(sum-1) > 10.0*REAL_EPSILON) {
#if 0
	    MSG("l_r[0]=%e, l_r[1]=%e, l_r[2]=%e\n",
		lambda0[0], lambda0[1], lambda0[2]);
#endif
	    ERROR_EXIT("%e = sum{lambda[i]} != 1\n", ABS(sum-1));
	  }

	  #if 0
	  MSG("Success! Newton needed %d iterations and failed %d times.\n",
	      iter, fail_count);
	  #endif
	  break;
	}
      }
      if (fail_count < N_LAMBDA(dim_of_mesh)+1) {
	REAL lmin = 0.0;
	k[iq] = -1;
	for (i = 0; i < N_LAMBDA(dim_of_mesh); i++) {
	  if (lambda[iq][i] < lambda_tolerance) {
	    if (lambda[iq][i] < lmin) {
	      k[iq] = i;
	      lmin = lambda[iq][i];
	    }
	  }
	}
      } else {
	WARNING("\a Newton failed totally. "
		"Maybe you have to lift the tolerance a little, "
		"or there is no solution.\n");
	k[iq] = -2;
      }
    }
    return;
  } else {
    for (iq = 0; iq < N; iq++) {
      k[iq] = world_to_coord(el_info, world[iq], lambda[iq]);
    }
    return;
  }
}

/* Return true if MESH carries our Lagrange parametric structure. */
bool _AI_is_lagrange_parametric(MESH *mesh)
{
  LAGRANGE_PARAM_DATA *data;

  return (mesh->parametric != NULL &&
	  (data = (LAGRANGE_PARAM_DATA *)mesh->parametric->data) != NULL &&
	  memcmp(data->magic, MAGIC_STRING, sizeof(data->magic)) == 0);
}

PARAM_STRATEGY _AI_lagrange_strategy(MESH *mesh)
{
  if (!_AI_is_lagrange_parametric(mesh)) {
    return (PARAM_STRATEGY)-1;
  }
  return ((LAGRANGE_PARAM_DATA *)mesh->parametric->data)->strategy;
}

static void _AI_use_lagrange_parametric(MESH *mesh, int degree,
					NODE_PROJECTION *n_proj,
					PARAM_STRATEGY strategy,
					FLAGS adm_flags,
					MESH *master);

static void inherit_lagrange_parametric(MESH *slave)
{
  FUNCNAME("inherit_lagrange_parametric");
  MESH                *master;
  PARAMETRIC          *m_parametric;
  LAGRANGE_PARAM_DATA *m_data;

  TEST_EXIT(slave, "No slave mesh given!\n");

  master = ((MESH_MEM_INFO *)slave->mem_info)->master;
  TEST_EXIT(master, "'%s' is not a slave mesh!\n", NAME(slave));

  m_parametric = master->parametric;
  TEST_EXIT(m_parametric, "'%s' is not a parametric mesh!\n", NAME(master));

  m_data = (LAGRANGE_PARAM_DATA *)m_parametric->data;

  /* Turn the slave mesh into a parametric mesh. This also transfers
   * coordinates and "edge_projections".
   */

  _AI_use_lagrange_parametric(slave,
			      m_data->degree,
			      m_data->n_proj, 
			      m_data->strategy,
			      m_data->coords->fe_space->admin->flags,
			      master);

  return;
}

static void unchain_lagrange_parametric(MESH *slave)
{
  PARAMETRIC          *s_parametric = slave->parametric;
  LAGRANGE_PARAM_DATA *s_data = (LAGRANGE_PARAM_DATA *)s_parametric->data;
  int dim, degree, deg_idx;
  
  dim    = slave->dim;
  degree = s_data->degree;

  deg_idx = degree >= DEGREE_GEN ? DEGREE_GEN-1 : degree-1;
  s_data->coords->refine_interpol = all_refine_interpol[dim][deg_idx];

  return;
}

/*******************************************************************************
 * use_lagrange_parametric(mesh, degree, n_proj, strategy):
 * Transforms the mesh to use parametric simplices of degree "degree".
 * All mesh elements which are affected by the node projection "n_proj"
 * will be deformed. The parameter "strategy" determines how elements will
 * become parametric. The following choices are available at the moment:
 *
 *  strategy == PARAM_ALL:
 *                 all elements of the mesh will be treated as parametric
 *                 elements, implying that determinants and Jacobeans will
 *                 be calculated at all quadrature points during assembly.
 *                 This is useful e.g. for triangulations of embedded
 *                 curved manifolds. Please note that during refinement a
 *                 parent element will be split along the surface defined
 *                 by the following equation
 *
 *                 lambda_0 = lambda_1
 *
 *                 defined in the nonlinear barycentric coords lambda_i.
 *                 For some meshes, this will create better shaped
 *                 simplices.
 *
 *  strategy == PARAM_CURVED_CHILDS:
 *                 only those elements of the mesh affected by "n_proj"
 *                 will be treated as parametric elements. This enables
 *                 some optimization during assembly. All children of the
 *                 original curved simplices will be determined as above,
 *                 implying that they too will be parametric.
 *
 *  strategy == PARAM_STRAIGHT_CHILDS:
 *                 only those elements of the mesh affected by "n_proj"
 *                 will be treated as parametric elements. The children of
 *                 parametric elements will only be parametric elements
 *                 if they too are affected by "n_proj". Parent elements
 *                 are split along straight lines or planes, which could
 *                 lead to degenerate elements. The purpose of this
 *                 algorithm is to keep the number of curved elements as
 *                 small as possible. This is useful if one only wishes to
 *                 use parametric elements to approximate a curved boundary
 *                 or interface manifold.
 *
 * If n_proj == NULL, then all elements which have just some
 * projection attached to it will become parametric. If n_proj !=
 * NULL, then only those elements are changed into curved elements
 * which have are subject to the same projection.
 *
 *****************************************************************************/

static void _AI_use_lagrange_parametric(MESH *mesh, int degree,
					NODE_PROJECTION *n_proj,
					PARAM_STRATEGY strategy,
					FLAGS adm_flags,
					MESH *master)
{
  FUNCNAME("use_lagrange_parametric");
  LAGRANGE_PARAM_DATA *data;
  PARAMETRIC     *parametric;
  DOF_REAL_D_VEC *coords;
  DOF_PTR_VEC    *edge_pr = NULL;
  const BAS_FCTS *lagrange = NULL;
  int            dim, i, deg_idx;
  MESH           *slave;
  bool           selective = n_proj != NULL;
  const FE_SPACE *fe_space;

  TEST_EXIT(mesh,"No fe_space given!\n");

  if (mesh->parametric != NULL) {
    WARNING("There is already a parametric structure defined on this mesh!\n");
  }

  dim = mesh->dim;

  TEST_EXIT((dim >= 0) && (dim <= DIM_MAX),
	    "Parametric elements of dimension %d "
	    "are not available for DIM_MAX == %d!\n", dim, DIM_MAX);

  TEST_EXIT((degree > 0) && (degree <= DEGREE_MAX),
	    "Only implemented for 1 <= degree <= %d.\n", DEGREE_MAX);

  deg_idx = degree >= DEGREE_GEN ? DEGREE_GEN-1 : degree-1;

  TEST_EXIT(strategy >= PARAM_ALL && strategy <= PARAM_STRAIGHT_CHILDS,
	    "Only strategy 0, 1, 2 are implemented!\n");

  /* Promote the strategy to PARAM_ALL if all elements have a
   * projection attached; do this only for mesh-hierarchies.
   */
  if (master) {
    MACRO_EL *mel;
    int not_all;
    
    not_all = false;
    if (strategy != PARAM_ALL) {
      for (mel = mesh->macro_els;
	   mel < mesh->macro_els + mesh->n_macro_el;
	   mel++) {
	if (mel->projection[0] == NULL ||
	    (selective && n_proj != mel->projection[0])) {
	  not_all = true;
	  break;
	}
      }
      if (!not_all) {
	strategy = PARAM_ALL;
      }
    }
  }
  
  if (dim > 0 && degree > 1 && strategy != PARAM_ALL) {
    int n_dof[N_NODE_TYPES] = { 0, };
    const FE_SPACE *edge_fe_space;
    
    if (dim > 1) {
      n_dof[EDGE] = 1;
    } else {
      n_dof[CENTER] = 1;
    }
    
    /* "edge_projections" belongs to a periodic DOF_ADMIN on periodic
     * meshes.
     */
    edge_fe_space = get_dof_space(mesh, "Edge dof fe_space", n_dof,
				  ADM_FLAGS_DFLT|ADM_PERIODIC);
    
    edge_pr = get_dof_ptr_vec("Edge projections", edge_fe_space);
    FOR_ALL_DOFS(edge_fe_space->admin, edge_pr->vec[dof] = NULL);
    free_fe_space(edge_fe_space);
  }

  lagrange = get_lagrange(dim, degree);
  fe_space = get_fe_space(mesh,
			  lagrange->name, lagrange, DIM_OF_WORLD, adm_flags);
  coords = get_dof_real_d_vec("Lagrange parametric coordinates", fe_space);
  coords->refine_interpol = all_refine_interpol[dim][deg_idx];
  coords->coarse_restrict = all_coarse_interpol[dim][deg_idx];

  data                   = MEM_CALLOC(1, LAGRANGE_PARAM_DATA);
  data->degree           = degree;
  data->strategy         = strategy;
  data->n_proj           = n_proj;
  data->coords           = coords; 
  data->edge_projections = edge_pr;
  data->n_local_coords   = fe_space->bas_fcts->n_bas_fcts;

  data->max_iter         = 1000;
  data->newton_tolerance = 5e-14;
  data->lambda_tolerance = -75*REAL_EPSILON;

  GET_PARAMETER(0, "parametric->lagrange->param_world_to_coord"
		"->newton->max_iter" , "%d", &data->max_iter);
  GET_PARAMETER(0, "parametric->lagrange->param_world_to_coord"
		"->newton->tolerance", "%f", &data->newton_tolerance);
  GET_PARAMETER(0, "parametric->lagrange->param_world_to_coord"
		"->lambda->tolerance", "%f", &data->lambda_tolerance);

  if (dim > 0 && degree > 1) {
    data->param_local_coords = MEM_CALLOC(lagrange->n_bas_fcts, REAL_D);
    data->i_am_affine = false;
  } else {
    data->param_local_coords = NULL;
    data->i_am_affine = true;
  }
  data->local_coords = strategy == PARAM_ALL ? data->param_local_coords : NULL;
  memcpy(data->magic, MAGIC_STRING, sizeof(data->magic));

  /* Inherit the data from the master to assure consistent coordinate
   * information for the entire mesh hierarchy.
   */

  if (master) {
#if DIM_MAX > 0
    if (dim == 0) {
      slave_fill_coords_0d(data);
      coords->refine_interpol = NULL;
# if DIM_MAX > 1
    } else if (dim == 1) {
      slave_fill_coordsY_1d(data);
      coords->refine_interpol = slave_refine_interpolY_1d;
#  if DIM_MAX > 2
    } else if (dim == 2) {
      slave_fill_coordsY_2d(data);
      coords->refine_interpol = slave_refine_interpolY_2d;
#  endif
# endif
    } else {
      ERROR_EXIT("Strange combinations of dimensions: %d / %d\n",
		 dim, master->dim);
    }
#endif
  } else {
    all_fill_coords[dim][deg_idx](data);
  }

  /* update mesh->bbox and mesh->diam */
  SET_DOW(REAL_MAX, mesh->bbox[0]);
  SET_DOW(REAL_MIN, mesh->bbox[1]);
  FOR_ALL_DOFS(coords->fe_space->admin,
	       REAL *value = coords->vec[dof];
	       for (i = 0; i < DIM_OF_WORLD; i++) {
		 mesh->bbox[0][i] = MIN(mesh->bbox[0][i], value[i]);
		 mesh->bbox[1][i] = MAX(mesh->bbox[1][i], value[i]);
	       });
  AXPBY_DOW(1.0, mesh->bbox[1], -1.0, mesh->bbox[0], mesh->diam);

  parametric          = MEM_CALLOC(1, PARAMETRIC);
  *parametric         = *all_lagrange[dim][deg_idx];
  parametric->data    = data;
  mesh->parametric    = parametric;
  parametric->not_all = degree == 1 || data->strategy != PARAM_ALL;

  /* Set inheritance function. */
  parametric->inherit_parametric = inherit_lagrange_parametric;
  parametric->unchain_parametric = unchain_lagrange_parametric;

  /* Now handle submeshes. Pass ourselves as "master" to the sub-meshes */

  if(mesh->dim > 0) {
    for (i = 0; i < ((MESH_MEM_INFO *)mesh->mem_info)->n_slaves; i++) {
      slave = ((MESH_MEM_INFO *)mesh->mem_info)->slaves[i];

      _AI_use_lagrange_parametric(slave, degree, n_proj,
				  strategy, adm_flags, mesh);
    }
  }
}

void use_lagrange_parametric(MESH *mesh, int degree,
			     NODE_PROJECTION *n_proj,
			     FLAGS flags)
{
  FUNCNAME("use_lagrange_parametric");
  PARAM_STRATEGY strategy;
  FLAGS adm_flags;

  if (_AI_is_lagrange_parametric(mesh)) {
    WARNING("The mesh already has a parametric structure! "
	    "A second call will likely corrupt your mesh. "
	    "Returning without change.\n");
    return;
  }

  TEST_EXIT(get_master(mesh) == NULL,
	    "ERROR: Parametric structures must be added on the "
	    "top-most master mesh of a sub-mesh hierarchy.\n");

  strategy = (PARAM_STRATEGY)(flags & ~PARAM_PERIODIC_COORDS);
  adm_flags = (flags & PARAM_PERIODIC_COORDS) ? ADM_PERIODIC : ADM_FLAGS_DFLT;
  _AI_use_lagrange_parametric(mesh, degree, n_proj, strategy, adm_flags, NULL);
}

/*******************************************************************************
 * get_lagrange_coords(mesh): Return the DOF_REAL_D_VEC coordinate vector
 * used to defined the Lagrange type parametric elements implemented above.
 * USE WITH CARE, ESPECIALLY IF YOU ARE CHANGING VALUES! Problems may arise
 * because elements are not recognized as curved elements unless the
 * internally used "edge_projections" vector is also set.
 ******************************************************************************/
DOF_REAL_D_VEC *get_lagrange_coords(MESH *mesh)
{
  FUNCNAME("get_lagrange_coords");
  
  TEST_EXIT(mesh, "No mesh given!\n");

  if (!_AI_is_lagrange_parametric(mesh)) {
    /* Return NULL rather than bailing out. Let the caller take the
     * proper actions.
     */
    return NULL;
  } else {
    return ((LAGRANGE_PARAM_DATA *)mesh->parametric->data)->coords;
  }
}


DOF_PTR_VEC *get_lagrange_edge_projections(MESH *mesh)
{
  FUNCNAME("get_lagrange_edge_projections");

  TEST_EXIT(mesh, "No mesh given!\n");

  if (!_AI_is_lagrange_parametric(mesh)) {
    /* Return NULL rather than bailing out. Let the caller take the
     * proper actions.
     */
    return NULL;
  } else {
    return ((LAGRANGE_PARAM_DATA *)mesh->parametric->data)->edge_projections;
  }
}

/** A more secure interface to the coordinate-vector: only the @b
 * values are copied, the application does not get access to the
 * DOF_REAL_D_VEC itself. This function makes sure that affine
 * elements remain affine. We also handle the case when we do not have
 * a real parameterisation but just affine linear elements with
 * quasi-parametric coordinates stored in the EL->new_coord.  The
 * degree of the "coords" vector has to match the degree of the
 * iso-parametric parameterisation. We also recompute mesh->diam here.
 *
 * @param[in] mesh The mesh to change the coordinates of.
 *
 * @param[in,out] coords The coordinate vector to install or where to
 *         store the mesh's current coordinates in.
 *
 * @param[in] dir Direction of the copy action: dir == 0 means to
 *         store the mesh's current coordinates in coords. dir == 1 means
 *         to install the coordinates specified by coords in the mesh, either
 *         using EL->new_coord or by copying the values into the internal
 *         coordinate vector.
 */
void copy_lagrange_coords(MESH *mesh, DOF_REAL_D_VEC *coords, bool tomesh)
{
  PARAMETRIC *parametric = mesh->parametric;
  int i, j;
  int dim = mesh->dim;

  if (tomesh) {
    SET_DOW(REAL_MAX, mesh->bbox[0]);
    SET_DOW(REAL_MIN, mesh->bbox[1]);
    FOR_ALL_DOFS(coords->fe_space->admin,
		 REAL *value = coords->vec[dof];
		 for (i = 0; i < DIM_OF_WORLD; i++) {
		   mesh->bbox[0][i] = MIN(mesh->bbox[0][i], value[i]);
		   mesh->bbox[1][i] = MAX(mesh->bbox[1][i], value[i]);
		 });
    AXPBY_DOW(1.0, mesh->bbox[1], -1.0, mesh->bbox[0], mesh->diam);
  }

  if (parametric) {
    LAGRANGE_PARAM_DATA *data;
  
    TEST_EXIT(_AI_is_lagrange_parametric(mesh),
	      "Parametric data has not type LAGRANGE_PARAM_DATA.\n");

    data = (LAGRANGE_PARAM_DATA *)mesh->parametric->data;

    TEST_EXIT(coords->fe_space->bas_fcts == data->coords->fe_space->bas_fcts,
	      "basis function mismatch.\n");

    data = (LAGRANGE_PARAM_DATA *)mesh->parametric->data;

    if (data->strategy == PARAM_ALL || tomesh == false) {  /* the easy case */
      if (tomesh) {
	dof_copy_d(coords, data->coords);
      } else {
	dof_copy_d(data->coords, coords);
      }
    } else {
      const BAS_FCTS   *bas_fcts = coords->fe_space->bas_fcts;
      const DOF_ADMIN  *admin    = coords->fe_space->admin;
      const REAL_B     *nodes    = LAGRANGE_NODES(bas_fcts);
      NODE_PROJECTION  **edge_pr = (NODE_PROJECTION **)data->edge_projections->vec;
      int              n0_edge_pr, node_e;
      DOF              dof[bas_fcts->n_bas_fcts];

      dof_copy_d(coords, data->coords);

      node_e = mesh->node[EDGE];
      n0_edge_pr = data->edge_projections->fe_space->admin->n0_dof[EDGE];

      /* We need a mesh-traversal to make sure that affine elements
       * remain affine.
       *
       * inefficient like hell, but so what
       */

      TRAVERSE_FIRST(mesh, -1, CALL_LEAF_EL) {
	int is_affine = true;
	
	for (i = 0; i < N_EDGES(dim); i++) {
	  if (edge_pr[el_info->el->dof[node_e+i][n0_edge_pr]] != NULL) {
	    is_affine = false;
	    break;
	  }
	}

	if (is_affine) {
	  GET_DOF_INDICES(bas_fcts, el_info->el, admin, dof);
	  for (i = N_VERTICES(dim); i < bas_fcts->n_bas_fcts; i++) {
	    AXEY_DOW(nodes[i][0], data->coords->vec[dof[0]],
		     data->coords->vec[dof[i]]);
	    for (j = 1; j < N_VERTICES(dim); j++) {
	      AXPY_DOW(nodes[i][j], data->coords->vec[dof[j]],
		       data->coords->vec[dof[i]]);
	    }
	  }
	}
      } TRAVERSE_NEXT();
    }
  } else {
    /* quasi-parametric case, "parametric" coords are stored in el->new_coord
     *
     * We do not use FILL_COORDS but fill the coordinates ourselves to
     * avoid too many copy instructions.
     */

    if (tomesh) {  /* coords -> mesh */
      const DOF_ADMIN *admin    = coords->fe_space->admin;
      int node_v, n0_v, v;
      
      node_v = mesh->node[VERTEX];
      n0_v   = admin->n0_dof[VERTEX];

      TRAVERSE_FIRST(mesh, -1, CALL_EVERY_EL_PREORDER|FILL_NEIGH) {
	if (el_info->level == 0) {
	  for (v = 0; v < N_VERTICES(dim); v++) {
	    COPY_DOW(coords->vec[el_info->el->dof[node_v+v][n0_v]],
		     *el_info->macro_el->coord[v]);		     
	  }
	}
	if (el_info->el->child[0]) {
	  DOF cdof = el_info->el->child[0]->dof[node_v+dim][n0_v];

	  TEST_EXIT(el_info->el->new_coord != NULL,
		    "el_info->el->new_coord == NULL");

	  COPY_DOW(coords->vec[cdof], el_info->el->new_coord);

	}
      } TRAVERSE_NEXT();

    } else {  /* mesh -> coords */
      const DOF_ADMIN *admin    = coords->fe_space->admin;
      int node_v, n0_v, v, dim = mesh->dim;
      
      node_v = mesh->node[VERTEX];
      n0_v   = admin->n0_dof[VERTEX];

      TRAVERSE_FIRST(mesh, -1, CALL_EVERY_EL_PREORDER) {
	if (el_info->level == 0) {
	  for (v = 0; v < N_VERTICES(dim); v++) {
	    COPY_DOW(*el_info->macro_el->coord[v],
		     coords->vec[el_info->el->dof[node_v+v][n0_v]]);
	  }
	}
	if (el_info->el->child[0]) {
	  DOF cdof = el_info->el->child[0]->dof[node_v+dim][n0_v];
	  if (el_info->el->new_coord) {
	    COPY_DOW(el_info->el->new_coord, coords->vec[cdof]);
	  } else {
	    DOF v0 = el_info->el->dof[node_v+0][n0_v];
	    DOF v1 = el_info->el->dof[node_v+1][n0_v];

	    AXPBY_DOW(0.5, coords->vec[v0], 0.5, coords->vec[v1],
		      coords->vec[cdof]);
	  }
	}
      } TRAVERSE_NEXT();
    }
  }
}

/* update(update data, dim, current iterate , update matrix true/false, rhs) */
static void nl_update(void *vdata, int dim, const REAL *lambda_n,
		      bool upd_DF, REAL *f)
{
  int i, j, k;
  ISP_DATA       *data = (ISP_DATA *)vdata;
  REAL_D       *coords = data->coords;
  BAS_FCTS      *bfcts = data->bfcts;
  REAL_D           x_n;
  const REAL  *grd_phi;

/* Fs: IR^(n+1) -> IR^(n), Fs(lambda) = sum_i{a_i \phi_i(lambda)},
 * a_i: world coords of the edges, lambda: barycentric coords,
 * phi_i: i-th lagrange basis fct.
 * f: IR^(n+1) -> IR^(n+1)
 * f(lambda):= / Fs(lambda)     - x \
 *             \ sum(lambda[i]) - 1 /
 */
  
  if (f) {
    param_coord_to_world(data->el_info, NULL, 1,
			 (const REAL_B *)lambda_n, &x_n);
    AXPBY_DOW(1.0, x_n, -1.0, data->x, f);
    f[DIM_OF_WORLD] = -1;
    for (i=0; i<DIM_OF_WORLD+1; i++) {
      f[DIM_OF_WORLD] += lambda_n[i];
    }
  }
  
  /* Df(lambda_n) */
  if (upd_DF) {
    for (j=0; j < DIM_OF_WORLD+1; j++) {
      for (i=0; i < DIM_OF_WORLD; i++) {
	data->Df[i][j] = 0;
	for (k=0; k < data->bfcts->n_bas_fcts; k++) {
	  grd_phi =  GRD_PHI(bfcts, k, lambda_n);
	  data->Df[i][j] += coords[k][i] * grd_phi[j];
	}
      }
      data->Df[DIM_OF_WORLD][j] = 1;
    }
  }
  return;
}

/* solve(solve data, dim, F, d) */
static int nl_solve(void *vdata, int dim, const REAL *rhs, REAL *d)
{
  ISP_DATA *data = (ISP_DATA *)vdata;
  REAL Df[DIM_OF_WORLD+1][DIM_OF_WORLD+1];
  REAL rhs_cp[DIM_OF_WORLD+1];

  memcpy(Df, data->Df, sizeof(Df));

  memcpy(rhs_cp, rhs, sizeof(rhs_cp));

  square_gauss((REAL *)Df, rhs_cp, d, DIM_OF_WORLD+1, 1);

  return 0;
}
