#include <alberta.h>

#include "reparam_cm.h"

static int dim=2, degree=1;
static const FE_SPACE *fe_space = nil;    /* fe-space */
static int N_con=6;                   /* number of linear constraints for */ 
                                      /* uniqueness, 6 for sphere */
static DOF_REAL_D_VEC *xneu = nil;    /* new coordinates on Gamma_h */
static DOF_REAL_D_VEC *phi_dh = nil;  /* conformal map Gamma_h -> S^2_h */
static DOF_REAL_VEC *lam_h = nil;     /* Lagrange factors for point */
                                      /*constraint of phi_dh; not yet used */
static REAL *rho;                     /* Lagrange factors for four integral */
                                      /* constraints; not yet used */
static DOF_INT_VEC *nbc = nil;        /* number of barycentric coords */
                                      /* to compute new x_dh */
                                      /* also flag field: if nbc[dof]==0 */
                                      /* then y_dh[dof] has not yet been */
                                      /* projected */
static DOF_INT_VEC **gdof = nil;      /* global dofs with which the barycentric */
                                      /* coords are associated */
static DOF_REAL_VEC **barcor = nil;   /* barycentric coordinates */
            /* finally, we will set */
            /* xnew[dof] = sum_l=0^nbc[dof] barcor[l][dof] * xold[gdof[l][dof]] */
            /* and analogously for other fields */

static DOF_REAL_D_VEC *hG1_dh = nil;      /* help vectors for phi_dh */
static DOF_REAL_D_VEC *hG2_dh = nil;
static DOF_REAL_D_VEC *hG3_dh = nil;
static DOF_REAL_VEC *hG1_h = nil;         /* help vectors for lam_h */
static DOF_REAL_VEC *hG2_h = nil;
static DOF_REAL_VEC *hG3_h = nil;
static REAL *rh1;                         /* help vectors for rho */
static REAL *rh2;
static REAL *rh3;

static DOF_REAL_D_VEC **m_dh = nil;       /* constraints M_0, ..., M_5 */
static DOF_MATRIX *A = nil;              /* stiffness matrix on Gamma_h */
static DOF_MATRIX *M = nil;              /* mass matrix on Gamma_h */


static void init_global()
{
  int l;
/* initialise global fields except fe_space */
  xneu   = get_dof_real_d_vec ("xneu_dh", fe_space);
  phi_dh = get_dof_real_d_vec ("phi_dh", fe_space);
  hG1_dh = get_dof_real_d_vec ("hG1_dh", fe_space);
  hG2_dh = get_dof_real_d_vec ("hG2_dh", fe_space);
  hG3_dh = get_dof_real_d_vec ("hG3_dh", fe_space);
  lam_h = get_dof_real_vec ("lam_h", fe_space);
  rho = MEM_ALLOC (N_con, REAL);
  rh1 = MEM_ALLOC (N_con, REAL);
  rh2 = MEM_ALLOC (N_con, REAL);
  rh3 = MEM_ALLOC (N_con, REAL);
  hG1_h = get_dof_real_vec ("hG1_h", fe_space);
  hG2_h = get_dof_real_vec ("hG2_h", fe_space);
  hG3_h = get_dof_real_vec ("hG3_h", fe_space);

  nbc = get_dof_int_vec ("nbc", fe_space);
  gdof = MEM_ALLOC (dim+1, DOF_INT_VEC *);
  for (l = 0; l < dim+1; l++) {
    gdof[l] = get_dof_int_vec ("gdof", fe_space);
  }
  barcor = MEM_ALLOC (dim+1, DOF_REAL_VEC *);
  for (l = 0; l < dim+1; l++){
    barcor[l] = get_dof_real_vec ("barcor", fe_space);
  }

  /* initialise constraints and matrices */
  m_dh = MEM_ALLOC (N_con, DOF_REAL_D_VEC *);
  for (l=0; l<N_con; l++)
    m_dh[l] = get_dof_real_d_vec ("m", fe_space);
  A = get_dof_matrix ("A", fe_space, fe_space);
  M = get_dof_matrix ("M", fe_space, fe_space);

  return;
}

static void terminate_global () 
{
  int l;

 /* free global fields except fe_space */
  free_dof_real_d_vec (xneu);
  free_dof_real_d_vec (phi_dh);
  free_dof_real_d_vec (hG1_dh);
  free_dof_real_d_vec (hG2_dh);
  free_dof_real_d_vec (hG3_dh);
  free_dof_real_vec (lam_h);
  MEM_FREE (rho, N_con, REAL);
  MEM_FREE (rh1, N_con, REAL);
  MEM_FREE (rh2, N_con, REAL);
  MEM_FREE (rh3, N_con, REAL);
  free_dof_real_vec (hG1_h);
  free_dof_real_vec (hG2_h);
  free_dof_real_vec (hG3_h);

  free_dof_int_vec (nbc);
  for (l = 0; l < dim+1; l++)
    free_dof_int_vec (gdof[l]);
  MEM_FREE (gdof, dim+1, DOF_INT_VEC *);
  for (l = 0; l < dim+1; l++)
    free_dof_real_vec (barcor[l]);
  MEM_FREE (barcor, dim+1, DOF_REAL_VEC *);

  /* free constraints and matrices */
  for (l=0; l<N_con; l++)
    free_dof_real_d_vec (m_dh[l]);
  MEM_FREE (m_dh, N_con, DOF_REAL_D_VEC *);
  free_dof_matrix (A);
  free_dof_matrix (M);

  return;
}

/***************************************/
/* assembling on Gamma_h and S^2_h     */
/***************************************/

static void assemble (const DOF_REAL_D_VEC *y_dh, 
		      const DOF_REAL_D_VEC *x_dh, 
		      const struct reparam_cm_data *crd) 
{
  int i, j, n, iq, k, l;

  static const QUAD *quad_G = nil;
  static const QUAD *quad_S = nil;
  const EL_REAL_D_VEC *(*get_loc_coords)(REAL_D result[],
					 const EL *, const DOF_REAL_D_VEC *);
  const EL_DOF_VEC *(*get_dof)(DOF *result, const EL *, 
			       const DOF_ADMIN *,const BAS_FCTS *thisptr);
  
  const REAL_B my_lambda = {0.25, 0.25, 0.5}; 
                  /* dummy point to evaluate the gradients */

  REAL_D x_loc[N_LAMBDA(dim)], gradphi[N_LAMBDA(dim)];
  const REAL_D *y_loc;
  REAL_D y_iq, e[2], normal;
  REAL det, area, h;
  EL_MATRIX *add_matrix = get_el_matrix(fe_space, fe_space, 0);
  EL_REAL_D_VEC *add_vector = get_el_real_d_vec(fe_space->bas_fcts);
  // EL_REAL_D_VEC *elvec_yloc = get_el_real_d_vec(fe_space->bas_fcts);
  REAL *const*addmat = add_matrix->data.real;
  REAL_D *addvec_d = add_vector->vec;
  
  TRAVERSE_STACK *stack = get_traverse_stack();
  EL_INFO *el_info;

  n = fe_space->bas_fcts->n_bas_fcts;
  get_dof = fe_space->bas_fcts->get_dof_indices;
  get_loc_coords = fe_space->bas_fcts->get_real_d_vec;
 
  add_matrix->n_row = add_matrix->n_col = n;
  add_vector->n_components = n;
//  elvec_yloc->n_components = N_LAMBDA(dim);

  /* assemble M and A on Gamma */
  if (!quad_G)
    quad_G = get_quadrature (dim, 2*degree);
  clear_dof_matrix(A);
  clear_dof_matrix(M);

  el_info = (EL_INFO *) traverse_first(stack, fe_space->mesh, -1, CALL_LEAF_EL);
  while (el_info) {
    el_info->mesh->parametric->init_element(el_info, el_info->mesh->parametric);
    el_info->mesh->parametric->grd_lambda(el_info, nil, 1, &my_lambda, 
					  &gradphi, NULL, &det);
    const EL_DOF_VEC *dof_G = (*get_dof)(NULL, el_info->el, fe_space->admin, NULL);
    (*get_loc_coords)(x_loc, el_info->el, x_dh);

    /* contribution to A */
    area = det/2.0;
    for (i = 0; i < n; i++)
      for (j = i; j < n; j++) /* symmetric */
        addmat[j][i] = addmat[i][j] = SCP_DOW (gradphi[i], gradphi[j]) * area;
    add_element_matrix(A, 1.0, add_matrix, 0, dof_G, dof_G, nil);

    /* contribution to M */
    for (i = 0; i < n; i++)
      for (j = i; j < n; j++) { /* symmetric */
        h = 0.0;
        for (iq = 0; iq < quad_G->n_points; iq++)
          h += quad_G->lambda[iq][i] * quad_G->lambda[iq][j] * quad_G->w[iq];
        h *= det;
        addmat[j][i] = addmat[i][j] = h;
      }
    
    add_element_matrix (M, 1.0, add_matrix, 0, dof_G, dof_G, nil);

    el_info = (EL_INFO *) traverse_next(stack, el_info);

  }
  free_traverse_stack(stack);

  free_el_matrix(add_matrix);
  


  /* assemble m_sk on S, k=0,1,2,3 */
  TEST_EXIT (N_con==6, "Remeshing with conformal map is only implemented for spheres involving 6 linear constraints; we have a different number!\n");


  if (!quad_S) {
    if (crd->quad_degree_on_S==0) {
      MSG ("no degree for quadrature on S provided; set to 4\n");
      quad_S = get_quadrature (dim, 4);
    }
    else
      quad_S = get_quadrature (dim, crd->quad_degree_on_S);
  }
  for (l = 0; l < N_con; l++)
    dof_set_d(0.0, m_dh[l]);

  el_info = (EL_INFO *) traverse_first(stack, fe_space->mesh,-1, 
				       CALL_LEAF_EL);
  while (el_info) {
    el_info->mesh->parametric->init_element(el_info, 
					    el_info->mesh->parametric);
    const EL_DOF_VEC *dof_S = (*get_dof)(NULL, el_info->el, 
					 fe_space->admin, nil);

    const EL_REAL_D_VEC *elvec_yloc = (*get_loc_coords)(NULL, el_info->el, 
							y_dh);
    y_loc = elvec_yloc->vec;

    /* compute det = |e[0] wedge e[1]| and area = det/2.0 */
    for (l = 0; l < DIM_OF_WORLD; l++) {
      e[0][l] = y_loc[1][l] - y_loc[0][l];
      e[1][l] = y_loc[2][l] - y_loc[0][l];
    }
    WEDGE_DOW(e[0], e[1], normal);
    det = NORM_DOW(normal);
    area = 0.5*det;

    /* contribution to m_dh[0] */
    area = det/2.0;
    for (i = 0; i < n; i++) {
      addvec_d[i][0] = area/3.0;
      addvec_d[i][1] = 0.0;
      addvec_d[i][2] = 0.0;
    }

    add_element_d_vec(m_dh[0], 1.0, add_vector, dof_S, nil);

    /* contribution to m_dh[1] */
    area = det/2.0;
    for (i = 0; i < n; i++) {
      addvec_d[i][0] = 0.0;
      addvec_d[i][1] = area/3.0;
      addvec_d[i][2] = 0.0;
    }
    
    add_element_d_vec(m_dh[1], 1.0, add_vector, dof_S, nil);

    /* contribution to m_dh[2] */
    area = det/2.0;
    for (i = 0; i < n; i++) {
      addvec_d[i][0] = 0.0;
      addvec_d[i][1] = 0.0;
      addvec_d[i][2] = area/3.0;
    }
    
    add_element_d_vec(m_dh[2], 1.0, add_vector, dof_S, nil);

    /* contribution to m_dh[3] */
    for (i = 0; i < n; i++)
      for (k = 0; k < DIM_OF_WORLD; k++)
	addvec_d[i][k] = 0.0;
    for (iq = 0; iq < quad_S->n_points; iq++) {
      eval_uh_d(y_iq, quad_S->lambda[iq], elvec_yloc, fe_space->bas_fcts);
      h = NORM_DOW(y_iq);
      AX_DOW(1.0/h, y_iq);
      for (i = 0; i < n; i++) {
        addvec_d[i][0] += quad_S->lambda[iq][i] * (-y_iq[1]) * quad_S->w[iq] * det;
        addvec_d[i][1] += quad_S->lambda[iq][i] * y_iq[0] * quad_S->w[iq] * det;
      }
    }
    add_element_d_vec(m_dh[3], 1.0, add_vector, dof_S, nil);

    /* contribution to m_dh[4] */
    for (i = 0; i < n; i++)
      for (k = 0; k < DIM_OF_WORLD; k++)
	addvec_d[i][k] = 0.0;
    for (iq = 0; iq < quad_S->n_points; iq++) {
      eval_uh_d(y_iq, quad_S->lambda[iq], elvec_yloc, fe_space->bas_fcts);
      h = NORM_DOW(y_iq);
      AX_DOW(1.0/h, y_iq);
      for (i=0; i<n; i++) {
	addvec_d[i][0] += quad_S->lambda[iq][i] * (-y_iq[2]) * quad_S->w[iq] * det;
	addvec_d[i][2] += quad_S->lambda[iq][i] * y_iq[0] * quad_S->w[iq] * det;
      }
    }
    add_element_d_vec(m_dh[4], 1.0, add_vector, dof_S, nil);

    /* contribution to m_dh[5] */
    for (i = 0; i < n; i++)
      for (k = 0; k < DIM_OF_WORLD; k++)
 	addvec_d[i][k] = 0.0;
    for (iq = 0; iq < quad_S->n_points; iq++) {
      eval_uh_d(y_iq, quad_S->lambda[iq], elvec_yloc, fe_space->bas_fcts);
      h = NORM_DOW(y_iq);
      AX_DOW(1.0/h, y_iq);
      for (i=0; i<n; i++) {
	addvec_d[i][1] += quad_S->lambda[iq][i] * (-y_iq[2]) * quad_S->w[iq] * det;
	addvec_d[i][2] += quad_S->lambda[iq][i] * y_iq[1] * quad_S->w[iq] * det;
      }
    }
    add_element_d_vec(m_dh[5], 1.0, add_vector, dof_S, nil);

    el_info = (EL_INFO *) traverse_next(stack, el_info);

  }
  free_traverse_stack(stack);
  free_el_real_d_vec(add_vector);
//  free_el_real_d_vec(elvec_yloc);
  

  return;
}

/******************************************/
/* compute conformal map Gamma_h -> S^2_h */
/******************************************/

struct my_update_N_data {
  REAL *uk; /* actual iterate uk=(phi,lam,rho), */
            /*filled in my_update_N, used in my_mat_vec_N */
};

static void my_update_N (void *upd, int D, const REAL *u, bool mat_flag, REAL *g) 
{
  /* computes g = F'(u) */
  int ct, k, l;
  struct my_update_N_data *up_data = (struct my_update_N_data *)upd;

  /* copy input u to hG1_dh, hG1_h, rh1, and fill up_data->uk */
  ct = 0;
  FOR_ALL_DOFS (fe_space->admin, 
		for(k=0; k<DIM_OF_WORLD; k++) {
		  hG1_dh->vec[dof][k] = u[ct];
		  up_data->uk[ct] = u[ct];
		  ct++;
		}
		);
  FOR_ALL_DOFS (fe_space->admin, 
		hG1_h->vec[dof] = u[ct];
                up_data->uk[ct] = u[ct];
		ct++;
		);
  for (l = 0; l < N_con; l++) {
    rh1[l] = u[ct];
    up_data->uk[ct] = u[ct];
    ct++;
  }

  if (g != nil) {
    /* compute F'(u) = (hG2_dh, hG2_h, rh2) */

    /* hG2_dh = A hG1_dh + hG1_h hG1_dh + sum_l rh1[l] m_dh[l] */
    dof_mv_d(0, A, NULL, hG1_dh, hG2_dh);
    FOR_ALL_DOFS(fe_space->admin,
      for(k = 0; k < DIM_OF_WORLD; k++) {
  	hG2_dh->vec[dof][k] += hG1_h->vec[dof] * hG1_dh->vec[dof][k];
	for (l = 0; l < N_con; l++)
          hG2_dh->vec[dof][k] += rh1[l] * m_dh[l]->vec[dof][k];
      }
    );

    /* hG2_h = ( 0.5 (|hG1_dh[dof]|^2 - 1.0) )_dof */
    FOR_ALL_DOFS(fe_space->admin, 
		 hG2_h->vec[dof] = 0.5 * (SCP_DOW (hG1_dh->vec[dof],
						   hG1_dh->vec[dof]) - 1.0); );

    /* rh2[l] = m_dh[l] . hG1_dh */
    for (l = 0; l < N_con; l++)
      rh2[l] = 0.0;
    FOR_ALL_DOFS(fe_space->admin,
      for (l = 0; l < N_con; l++)
        rh2[l] += SCP_DOW(m_dh[l]->vec[dof], hG1_dh->vec[dof]);
    );

    /* copy to output g */
    ct = 0;
    FOR_ALL_DOFS(fe_space->admin, 
  		for(k = 0; k < DIM_OF_WORLD; k++) {
  		  g[ct] = hG2_dh->vec[dof][k];
  		  ct++;
  		}
  		);
    FOR_ALL_DOFS(fe_space->admin, 
  		g[ct] = hG2_h->vec[dof];
  		ct++;
  		);
    for (l = 0; l < N_con; l++) {
      g[ct] = rh2[l];
      ct++;
    }
  }

  return;
}

struct my_mat_vec_gmres_data {
  REAL *uk; /* actual iterate uk=(phi,lam,rho), */
            /* filled in my_update_N, used for computing F''(u) */
};

static int my_mat_vec_gmres (void *ud, int D, const REAL *input, REAL *output) 
{
  /* compute output = F''(uk) input */
  /* uk, actual iterate, must be available in ud->uk */
  int ct, k, l;
  struct my_mat_vec_gmres_data *mv_data = (struct my_mat_vec_gmres_data *)ud;

  /* copy input to (hG1_dh, hG1_h, rh1) and mv_data->uk to (hG3_dh, hG3_h, rh3) */
  ct = 0;
  FOR_ALL_DOFS (fe_space->admin, 
    for(k = 0; k < DIM_OF_WORLD; k++) {
      hG1_dh->vec[dof][k] = input[ct];
      hG3_dh->vec[dof][k] = mv_data->uk[ct];
      ct++;
    }
  );
  FOR_ALL_DOFS(fe_space->admin, 
    hG1_h->vec[dof] = input[ct];
    hG3_h->vec[dof] = mv_data->uk[ct];
    ct++;
  );
  for (l = 0; l < N_con; l++) {
    rh1[l] = input[ct];
    rh3[l] = mv_data->uk[ct];
    ct++;
  }

  /* hG2_dh = (A+diag(hG3_h)) hG1_dh  */
  /*        + (diag(hG3_dh[.][0]),diag(hG3_dh[.][1]),diag(hG3_dh[.][2]))^T hG1_h */
  /*        + sum_l m_dh[l] rh1[l] */
  dof_mv_d (0, A, NULL, hG1_dh, hG2_dh);
  FOR_ALL_DOFS (fe_space->admin,
    for (k = 0; k < DIM_OF_WORLD; k++) {
      hG2_dh->vec[dof][k] += hG3_h->vec[dof] * hG1_dh->vec[dof][k] 
	+ hG3_dh->vec[dof][k] * hG1_h->vec[dof];
      for (l = 0; l < N_con; l++)
	hG2_dh->vec[dof][k] += m_dh[l]->vec[dof][k] * rh1[l];
    }
  );

  /* hG2_h = (diag(hG3_dh[.][0]),diag(hG3_dh[.][1]),diag(hG3_dh[.][2])) hG1_dh */
  FOR_ALL_DOFS(fe_space->admin, 
	       hG2_h->vec[dof] = SCP_DOW(hG3_dh->vec[dof], hG1_dh->vec[dof]); );

  /* rh2[l] = m_dh[l] . hG1_dh */
  for (l = 0; l < N_con; l++)
    rh2[l] = 0.0;
  FOR_ALL_DOFS(fe_space->admin,
    for (l = 0; l < N_con; l++)
      rh2[l] += SCP_DOW(m_dh[l]->vec[dof], hG1_dh->vec[dof]);
  );

  /* copy (hG2_dh, hG2_h, rh2) to output */
  ct = 0;
  FOR_ALL_DOFS(fe_space->admin, 
    for(k = 0; k < DIM_OF_WORLD; k++) {
      output[ct] = hG2_dh->vec[dof][k];
      ct++;
    }
  );
  FOR_ALL_DOFS(fe_space->admin, 
    output[ct] = hG2_h->vec[dof];
    ct++;
  );
  for (l = 0; l < N_con; l++) {
    output[ct] = rh2[l];
    ct++;
  }

  return 0;
}

struct my_uzawa_data {
  REAL *uk; /* actual iterate uk=(phi,lam,rho), */
            /* filled in my_update_N, used for computing F''(u) */
  int max_iter, info;
  REAL tolerance;
  int max_iter_ell, info_ell;
  REAL tolerance_ell;
};

struct my_mv_Ax_data {
  DOF_REAL_D_VEC *h1;
  DOF_REAL_D_VEC *h2;
  DOF_REAL_VEC *lam;
};

static int my_mv_Ax (void *ud, int D, const REAL *in, REAL *out) 
{
  /* realises matrix-vector multiplication A x where                         */
  /*     (stiffness + diag(lam),        0            ,        0            ) */
  /* A = (        0            ,stiffness + diag(lam),        0            ) */
  /*     (        0            ,        0            ,stiffness + diag(lam)) */
  int ct, k;
  struct my_mv_Ax_data *mAxd = (struct my_mv_Ax_data *)ud;

  ct = 0.0;
  FOR_ALL_DOFS (fe_space->admin,
		for(k=0; k<DIM_OF_WORLD; k++) {
		  mAxd->h1->vec[dof][k] = in[ct];
		  ct++;
		} 
		);

  dof_mv_d(0, A, NULL, mAxd->h1, mAxd->h2); /* A is the stiffness matrix */
  FOR_ALL_DOFS (fe_space->admin,
		for (k = 0; k < DIM_OF_WORLD; k++)
		  mAxd->h2->vec[dof][k] += mAxd->lam->vec[dof] 
		    * mAxd->h1->vec[dof][k];
		);

  ct = 0.0;
  FOR_ALL_DOFS (fe_space->admin,
		for(k = 0; k < DIM_OF_WORLD; k++) {
		  out[ct] = mAxd->h2->vec[dof][k];
		  ct++;
		} 
		);

  return 0;
}

static int my_uzawa (void *ud, int D, const REAL *input, REAL *output) 
{
  /* to solve: (A B^T // B 0) output = input                                  */
  /* algorithm from Braess: 'Finite Elemente'                                 */
  /* notation: uk = (phi,lam,rho) actual Newton iterate                       */
  /*                                                                          */
  /*      (stiffness + diag(lam),        0            ,        0            ) */
  /*  A = (        0            ,stiffness + diag(lam),        0            ) */
  /*      (        0            ,        0            ,stiffness + diag(lam)) */
  /*                                                                          */
  /*      (diag(phi_.1), diag(phi_.2), diag(phi_.3))   . stands for dofs,     */
  /*      (              m_dh[0]^T                 )   numbers 1,2,3 for dim  */
  /*  B = (                 |                      )                          */
  /*      (              m_dh[5]^T                 )                          */
  FUNCNAME ("my_uzawa");
  int iter=0, ct, k, l, N;
  struct my_uzawa_data *uz_data = (struct my_uzawa_data *)ud;
  static DOF_REAL_VEC *d_h = nil; REAL *rd;   /* d, next conjugate direction */
  static DOF_REAL_VEC *q_h = nil; REAL *rq;   /* q, */
                                              /* defect(hG1_h,rh1)- B(hG2_h,rh2) */
  static DOF_REAL_D_VEC *p_dh = nil;          /* p = B^T d, help vector */
  static DOF_REAL_D_VEC *h_dh = nil;          /* h = A^-1 p, help vector */
  static DOF_REAL_D_VEC *h1_dh = nil;         /* two more help vectors */
  static DOF_REAL_D_VEC *h2_dh = nil;         /* for Ax mutiplication */
  REAL alpha, beta, res;
  static struct my_mv_Ax_data *mAxd = nil;
  static REAL *in = nil;
  static REAL *out = nil;
  static OEM_DATA *my_oem = nil;

  /* allocate space and initialise structures */
  rd = MEM_ALLOC (N_con, REAL);
  rq = MEM_ALLOC (N_con, REAL);
  N = fe_space->admin->size_used;
  if (d_h == nil) {
    /* vectors */
    d_h = get_dof_real_vec ("d", fe_space);
    q_h = get_dof_real_vec ("q", fe_space);
    p_dh = get_dof_real_d_vec ("p", fe_space);
    h_dh = get_dof_real_d_vec ("h", fe_space);
    h1_dh = get_dof_real_d_vec ("h1", fe_space);
    h2_dh = get_dof_real_d_vec ("h2", fe_space);

    /* data for matrix-vector multiplication Ax */
    mAxd = MEM_ALLOC (1, struct my_mv_Ax_data);
    mAxd->h1  = h1_dh;
    mAxd->h2  = h2_dh;
    mAxd->lam = hG3_h;
    in  = MEM_ALLOC (N*DIM_OF_WORLD, REAL);
    out = MEM_ALLOC (N*DIM_OF_WORLD, REAL);

    /* data for solver of Ax=y */
    my_oem = MEM_ALLOC (1, OEM_DATA);
    my_oem->mat_vec      = my_mv_Ax;
    my_oem->mat_vec_data = mAxd;
    my_oem->mat_vec_T         = nil;
    my_oem->mat_vec_T_data    = nil;
    my_oem->left_precon       = nil;
    my_oem->left_precon_data  = nil;
    my_oem->right_precon      = nil;
    my_oem->right_precon_data = nil;
    my_oem->scp               = nil;
    my_oem->scp_data          = nil;
    my_oem->ws           = nil;
    my_oem->tolerance    = uz_data->tolerance_ell;
    my_oem->restart      = 20;
    my_oem->max_iter     = uz_data->max_iter_ell;
    my_oem->info         = uz_data->info_ell;
  }

  /* copy input       to (hG1_dh, hG1_h, rh1) (rhs, const),              */
  /*      output      to (hG2_dh, hG2_h, rh2) (initial guess, changes,   */
  /*                                               later copied back),   */
  /* and  uz_data->uk to (hG3_dh, hG3_h, rh3) (=(phi,lam,rho), const,    */
  /*                                             actual Newton iterate). */
  ct = 0;
  FOR_ALL_DOFS (fe_space->admin, 
    for(k = 0; k < DIM_OF_WORLD; k++) {
      hG1_dh->vec[dof][k] = input[ct];
      hG2_dh->vec[dof][k] = output[ct];
      hG3_dh->vec[dof][k] = uz_data->uk[ct];
      ct++;
    }
  );
  FOR_ALL_DOFS (fe_space->admin, 
    hG1_h->vec[dof] = input[ct];
    hG2_h->vec[dof] = output[ct];
    hG3_h->vec[dof] = uz_data->uk[ct];
    ct++;
  );
  for (l = 0; l < N_con; l++) {
    rh1[l] = input[ct];
    rh2[l] = output[ct];
    rh3[l] = uz_data->uk[ct];
    ct++;
  }

  /* solve hG2_dh^1 = A^-1 (hG1_dh - B^T (hG2_h,rh2)^0) */
  ct = 0;
  FOR_ALL_DOFS (fe_space->admin,
		for(k = 0; k < DIM_OF_WORLD; k++) {
		  in[ct] = hG1_dh->vec[dof][k] 
		    - hG3_dh->vec[dof][k]*hG2_h->vec[dof];
		  for (l = 0; l < N_con; l++)
		    in[ct] -= rh2[l] * m_dh[l]->vec[dof][k];
		  out[ct] = hG2_dh->vec[dof][k];
		  ct++;
		}
		);
  while (ct < DIM_OF_WORLD*N) {
    in[ct] = out[ct] = 0.0;
    ct++;
  }
  oem_bicgstab (my_oem, DIM_OF_WORLD*N, (const REAL *)in, out);
  ct = 0;
  FOR_ALL_DOFS (fe_space->admin,
    for(k = 0; k < DIM_OF_WORLD; k++)
      hG2_dh->vec[dof][k] = out[ct];
  );

  /* set d^1 = -q^1 = B hG2_dh^1 - (hG1_h, rh1); h^1=0 */
  FOR_ALL_DOFS (fe_space->admin,
    d_h->vec[dof] = SCP_DOW (hG3_dh->vec[dof], hG2_dh->vec[dof]) - hG1_h->vec[dof];
    q_h->vec[dof] = -d_h->vec[dof];
  );
  for (l = 0; l < N_con; l++) {
    rd[l] = dof_dot_d (m_dh[l], hG2_dh) - rh1[l];
    rq[l] = - rd[l];
  }
  dof_set_d (0.0, h_dh);

  /* res = q^1.q^1 */
  res = dof_dot (q_h, q_h) + rq[0]*rq[0] + rq[1]*rq[1] + rq[2]*rq[2] + rq[3]*rq[3];

  if (uz_data->info >= 2) {
    MSG ("iter. |     residual \n");
    MSG ("%5d | %e \n", iter, res);
  }

  while (iter < uz_data->max_iter && sqrt(res) > uz_data->tolerance) {
    /* p^k = B^t d^k */
    FOR_ALL_DOFS (fe_space->admin,
		  for (k = 0; k < DIM_OF_WORLD; k++) {
		    p_dh->vec[dof][k] = hG3_dh->vec[dof][k]*d_h->vec[dof];
		    for (l = 0; l < N_con; l++)
		      p_dh->vec[dof][k] += rd[l] * m_dh[l]->vec[dof][k];
		  }
    );

    /* h^k = A^-1 p^k */
    ct = 0;
    FOR_ALL_DOFS (fe_space->admin,
      for(k = 0; k < DIM_OF_WORLD; k++) {
	in[ct] = p_dh->vec[dof][k];
	/*        out[ct] = h_dh->vec[dof][k]; */
        out[ct] = 0.0;
	ct++;
      }
    );
    while (ct < DIM_OF_WORLD*N) {
      in[ct] = out[ct] = 0.0;
      ct++;
    }
    oem_bicgstab (my_oem, DIM_OF_WORLD*N, (const REAL *)in, out);
    ct = 0;
    FOR_ALL_DOFS (fe_space->admin,
      for(k = 0; k < DIM_OF_WORLD; k++)
	h_dh->vec[dof][k] = out[ct];
    );

    /* alpha = q^k.q^k / p^k.d^k */
    alpha = res / dof_dot_d (p_dh, h_dh);

    /* (hG2_h,rh2)^k = (hG2_h,rh2)^(k-1) + alpha d^k */
    /* hG2_dh^k      = hG2_h^(k-1) - alpha h^k */
    FOR_ALL_DOFS (fe_space->admin,
      hG2_h->vec[dof] += alpha*d_h->vec[dof];
      for (k = 0; k < DIM_OF_WORLD; k++)
	hG2_dh->vec[dof][k] -= alpha*h_dh->vec[dof][k];
    );
    for (l = 0; l < N_con; l++)
      rh2[l] += alpha*rd[l];

    /* q^(k+1) = (hG1_h,rh1) - B hG2_dh^k */
    FOR_ALL_DOFS(fe_space->admin, 
		 q_h->vec[dof] = hG1_h->vec[dof] - SCP_DOW (hG3_dh->vec[dof],
							    hG2_dh->vec[dof]); );
    for (l = 0; l < N_con; l++)
      rq[l] = rh1[l] - dof_dot_d (m_dh[l], hG2_dh);

    /* res  = q^(k+1).q^(k+1) */
    /* beta = || q^(k+1) ||^2 / || q^k ||^2 = res_new/res_old */
    beta = 1.0 / res;
    res = dof_dot (q_h, q_h) + rq[0]*rq[0] 
      + rq[1]*rq[1] + rq[2]*rq[2] + rq[3]*rq[3];
    beta *= res;

    /* d^(k+1) = - q^(k+1) + beta d^k */
    FOR_ALL_DOFS(fe_space->admin, 
		 d_h->vec[dof] = - q_h->vec[dof] + beta * d_h->vec[dof]; );
    for (l = 0; l < N_con; l++)
      rd[l] = -rq[l] + beta * rd[l];

    iter++;
    if (uz_data->info >= 2)
      MSG ("%5d | %e \n", iter, res);
  }

  /* copy solution (hG2_dh, hG2_h, rh2) to output */
  ct = 0;
  FOR_ALL_DOFS (fe_space->admin, 
    for(k = 0; k < DIM_OF_WORLD; k++) {
      output[ct] = hG2_dh->vec[dof][k];
      ct++;
    }
  );
  FOR_ALL_DOFS(fe_space->admin, 
    output[ct] = hG2_h->vec[dof];
    ct++;
  );
  for (l = 0; l < N_con; l++) {
    output[ct] = rh2[l];
    ct++;
  }

  /* free allocated space */
  MEM_FREE (rq, N_con, REAL);
  MEM_FREE (rd, N_con, REAL);
  return iter;
}

struct my_solve_N_data {
  REAL *uk; /* actual iterate uk=(phi,lam,rho), */
            /* filled in my_update_N, used for F''(u) */
  const struct reparam_cm_data *crd; /* contains informations for the solver */
};

static int my_solve_N (void *sod, int D, const REAL *g, REAL *d) 
{
  /* solves F''(u) d = F'(u), g=F'(u) */
  struct my_solve_N_data *so_data = (struct my_solve_N_data *)sod;
  int iter=0, ct;
  static REAL *my_input = nil;
  static REAL *my_output = nil;
  FUNCNAME ("my_solve_N");

  /* prepare input = g=F'(u) and initialise output with d */
  if (my_input == nil) {
    my_input = MEM_ALLOC (D, REAL);
    my_output = MEM_ALLOC (D, REAL);
  }
  for (ct = 0; ct < D; ct++) {
    my_input[ct] = g[ct];
    my_output[ct] = d[ct];
  }

  switch (so_data->crd->spp_solver) {
  case 0:
    MSG ("no solver for the saddle point problems provided\n");
    break;
  case 1:
    /* use GMRES; a pointer OEM_DATA *spp_oem must be provided in   */
    /*               so_data->crd->spp_solver_data                  */
    /* containing values for tolerance, max_iter, info, and restart */
    if (!so_data->crd->spp_gmres_data)
      MSG ("data for saddle point solver (gmres) are not provided");

    else { /* assume that spp_solver_data->spp_oem are provided */
      /* provide data for matrix-vector multiplication F''(u) d */
      static struct my_mat_vec_gmres_data *mv_data = nil;
      if (mv_data == nil) {
        mv_data = MEM_ALLOC (1, struct my_mat_vec_gmres_data);
        mv_data->uk = so_data->uk;
      }
      so_data->crd->spp_gmres_data->spp_oem->mat_vec = my_mat_vec_gmres;
      so_data->crd->spp_gmres_data->spp_oem->mat_vec_data = mv_data;

      /* solve, call gmres */
      iter = oem_gmres(so_data->crd->spp_gmres_data->spp_oem, D, 
		       my_input, my_output);
    }
    break;
  case 2: /* UZAWA with conjugate directions, cp. Braess: 'Finite Elemente' */
    if (!so_data->crd->spp_uzawa_data)
      MSG ("data for saddle point solver (gmres) are not provided");

    else {
      static struct my_uzawa_data *uz_data = nil;
      if (uz_data == nil) {
	uz_data = MEM_ALLOC (1, struct my_uzawa_data);
	uz_data->uk            = so_data->uk;
        uz_data->max_iter      = so_data->crd->spp_uzawa_data->max_iter;
        uz_data->info          = so_data->crd->spp_uzawa_data->info;
        uz_data->tolerance     = so_data->crd->spp_uzawa_data->tolerance;
        uz_data->max_iter_ell  = so_data->crd->spp_uzawa_data->max_iter_ell;
        uz_data->info_ell      = so_data->crd->spp_uzawa_data->info_ell;
        uz_data->tolerance_ell = so_data->crd->spp_uzawa_data->tolerance_ell;
      }
      iter = my_uzawa (uz_data, D, my_input, my_output);
    }
    break;
  default:
    MSG ("this solver for the saddle point problem is not yet available\n");
    iter = -1;
    break;
  }

  /* transfer result back into d */
  for (ct = 0; ct < D; ct++)
    d[ct] = my_output[ct];

  return iter;
}

static REAL my_norm_N (void *nod, int D, const REAL *f) 
{
  /* Euclidean norm */
  int ct;
  REAL r = 0.0;
  for (ct = 0; ct < D; ct++)
    r += f[ct]*f[ct];

  return sqrt(r);
}

static void compute_phi (const DOF_REAL_D_VEC *y_dh, 
			 const struct reparam_cm_data *crd) 
{
  /* use Newton iteration to compute solution to F'(phi_dh, lam_h, rho) = 0 */
  int N = -1, D, iter, ct, k, l;
  NLS_DATA my_nls = {nil};
  static struct my_update_N_data *upd = nil;
  static struct my_solve_N_data *sod = nil;
  static REAL *uk = nil; /* actual iterate (phi,lam,rho), vector of size D */
  REAL ntol = 1.0e-5;
  int niter = 100, ninfo = 0, nrestart = 20;

  /* fill my_solve_N_data */
  N = fe_space->admin->size_used;
  D = N_con*(N+1);
  if (uk == nil)
    uk = MEM_ALLOC(D, REAL);
  if (upd == nil) {
    upd = MEM_ALLOC (1, struct my_update_N_data);
    upd->uk = uk;
  }
  if (sod == nil) {
    sod = MEM_ALLOC (1, struct my_solve_N_data);
    sod->uk = uk;
    sod->crd = crd;
  }

  /* fill nls data */
  my_nls.update      = my_update_N;
  my_nls.update_data = upd;
  my_nls.solve       = my_solve_N;
  my_nls.solve_data  = sod;
  my_nls.norm        = my_norm_N;
  my_nls.norm_data   = nil;
  if (crd->ns_data) {
    my_nls.tolerance = crd->ns_data->tolerance;
    my_nls.restart   = crd->ns_data->restart;
    my_nls.max_iter  = crd->ns_data->max_iter;
    my_nls.info      = crd->ns_data->info;
  } else {
    my_nls.tolerance = ntol;
    my_nls.restart   = nrestart;
    my_nls.max_iter  = niter;
    my_nls.info      = ninfo;
  }

  /* initial guess for uk: vertices from grid on S, zero */
  ct = 0;
  FOR_ALL_DOFS(fe_space->admin, 
    for(k = 0; k < DIM_OF_WORLD; k++) {
      uk[ct] = y_dh->vec[dof][k];
      ct++;
    }
  );
  while (ct < D) {
    uk[ct] = 0.0;
    ct++;
  }

  /* start newtonsolver */
  switch (crd->newton_solver) {
  case 1:
    iter = nls_newton(&my_nls, D, uk);
    break;
  default:
    MSG("Newton solver not available\n");
    break;
  }

  /* put result to phi_dh, lam_h, rho */
  ct = 0;
  FOR_ALL_DOFS(fe_space->admin, 
    for(k = 0; k < DIM_OF_WORLD; k++) {
      phi_dh->vec[dof][k] = uk[ct];
      ct++;
    }
  );
  FOR_ALL_DOFS(fe_space->admin,
    lam_h->vec[dof] = uk[ct];
    ct++;
  );
  for (l = 0; l < N_con; l++) {
    rho[l] = uk[ct];
    ct++;
  }

  return;
}

/******************************************/
/* project nice grid on S^2_h to Gamma_h  */
/******************************************/

static int proj_to_el (const REAL_D y, const REAL_D *p, REAL_B *lambda) 
{
  /* given a triangle with vertices p[0],p[1],p[2] on S^2 */
  /* try to project the point y on S^2 in direction -n with n */
  /* external (to S^2) unit normal                               */
  /* gives back 0 if successful, and then lambda contains */
  /* the barycentric coordinates */
  /* otherwise it gives back 1 */
  /* Variables:
     p[k]:  coordinates of el vertex k (local dof, k=0,1,2), provided;
     e[k]:  edge vector p[k+2] - p[k+1] (indices mod 3);
     n:     normal of el, (e[0] wedge -e[1]) / its norm;
     mu[k]: conormal, (e[k] wedge normal) / its norm;
     pr:    projection of y to hyperplane of el,
            pr = y - r n  with  r = y.n - p[0].n;
     m[k]:  vector pr - p[k+1];
     d[k]:  distance of pr to e[k];
     s[k]:  distance of p[k] to e[k];
  */
  int k,l;
  REAL d[3], s[3], r, normn;
  REAL_D e[3], m[3], mu[3], n, pr, h;

  /* compute the e[k] */
  for (k = 0; k < 3; k++)
    for (l = 0; l < DIM_OF_WORLD; l++)
      e[k][l] = p[(k+2)%3][l] - p[(k+1)%3][l];

  /* compute n and the mu */
  WEDGE_DOW (e[0], e[1], n);
  normn = NORM_DOW (n);
  AX_DOW (1.0/normn, n);
  for (k = 0; k < 3; k++) {
    WEDGE_DOW (e[k], n, mu[k]);
    r = NORM_DOW (mu[k]);
    AX_DOW (1.0/r, mu[k]);
  }

  r = SCP_DOW (y, n); /* check whether we are on the right side */
  if (r >= 0) {
    /* project y to the hyperplane of the triangle */
    r = r - SCP_DOW (p[0], n);
    for (l = 0; l < DIM_OF_WORLD; l++)
      pr[l] = y[l] - r*n[l];

    /* for every edge e[k]: test if m[k].mu[k] = (pr-p[k+1]).mu[k] <=0 */
    /* (necessary for pr lying on el) */
    for (l = 0; l < DIM_OF_WORLD; l++)
      m[0][l] = pr[l] - p[1][l];
    if (SCP_DOW (m[0], mu[0]) <=0) { /* test edge e[0] */
      for (l = 0; l < DIM_OF_WORLD; l++)
        m[1][l] = pr[l] - p[2][l];
      if (SCP_DOW (m[1], mu[1]) <=0) { /* test edge e[1] */
        for (l = 0; l < DIM_OF_WORLD; l++)
          m[2][l] = pr[l] - p[0][l];
        if (SCP_DOW (m[2], mu[2]) <=0) { /* test edge e[2] */

          /* pr is lying on el */
	  /* compute barycentric coords lambda[k] = d[k] / s[k] where */
	  /* d[k] = dist of pr to e[k] */
	  /* s[k] = dist of p[k] to e[k] */

          for (k = 0; k < 3; k++) {
	    r = NORM_DOW (e[k]);
	    /* compute d[k] = |m[k+1] wedge e[k]/|e[k]|| */
	    WEDGE_DOW (m[(k+1)%3], e[k], h);
	    d[k] = NORM_DOW (h) / r;
	    /* compute s[k] = |e[k-1] wedge e[k]/|e[k]|| = normn/|e[k]|*/
	    s[k] = normn / r;
	    if (s[k] <= 1.0e-15)
	      (*lambda)[k] = 0.0;
	    else
	      (*lambda)[k] = d[k] / s[k];
	  }

	  return 0; /* successful */
        }
      }
    }
  }
  return 1; /* not successful */
}

static int project_nice_mesh (const DOF_REAL_D_VEC *y_dh, 
			      const struct reparam_cm_data *crd) 
{
  /* projects mesh points y_dh of the nice mesh on S^2 */
  /*     to the mesh phi(G_h) on S^2; */
  /* if a triangle is met: */
  /*   save the barycentric coords of the projection point in */
  /*                   barcor[0],barcor[1],barcor[2] */
  /*   and the global dofs corresponding to the triangle vertices in */
  /*                   gdof[0],gdof[1],gdof[2] */
  /*   set nbc[dof]=3 */
  /* if an edge is met: */
  /*   save the barycentric coords of the projection point in */
  /*                    barcor[0],barcor[1] */
  /*   and the global dofs corresponding to the edge vertices in */
  /*                    gdof[0],gdof[1] */
  /*   set nbc[dof]=2 */

  /* nbc->vec[dof]==0 means, the dof has not yet been treated */
  /* two runs over the mesh: */
  /* first, consider dofs of every element el and try to project them to el */
  /* second, globally handle remaining dofs */
  /* after: handle points y_dh->vec[dof] that lie not over an element */
  /*                       (project to closest edge) */

  FUNCNAME ("project_nice_mesh");
  int k, l, ctD, ctN, ctB, ctE;
  REAL_D p_loc[3];
  REAL_B bary_coords;

  REAL_D v,e,w,e_target,v_target;
  REAL d, d_min, len, ht, ht_target=0.5, lam0, lam1;
  DOF dof0=0,dof1=1;

  TRAVERSE_STACK *stack = get_traverse_stack();
  EL_INFO *el_info;
  EL * el;

//  static const REAL_D *(*get_loc_coords)(const EL *, const DOF_REAL_D_VEC *,				 REAL_D *); 
//  static const DOF *(*get_dof)(const EL *, const DOF_ADMIN *, DOF *);
  const EL_REAL_D_VEC *(*get_loc_coords)(REAL_D result[],
					 const EL *, const DOF_REAL_D_VEC *);
  const EL_DOF_VEC *(*get_dof)(DOF *result, const EL *, 
			       const DOF_ADMIN *,const BAS_FCTS *thisptr);
  

  FOR_ALL_DOFS (fe_space->admin, nbc->vec[dof] = 0; ); /* initialise flag field */

  get_dof = fe_space->bas_fcts->get_dof_indices;
  get_loc_coords = fe_space->bas_fcts->get_real_d_vec; 

  ctD=ctN=0;
  /* first run */
  el_info = (EL_INFO *)traverse_first (stack, fe_space->mesh, -1, 
				       CALL_LEAF_EL|FILL_NEIGH);
  while (el_info) {
    /*el_info->mesh->parametric->init_element(el_info,                */
    /*                                    el_info->mesh->parametric); */

    //   const DOF *dof_y = (*get_dof)(el_info->el, fe_space->admin, nil);
    const EL_DOF_VEC *dof_y = (*get_dof)(NULL, el_info->el, fe_space->admin, NULL);
   
    /* dof_y->vec[k] is global dof of local dof k */

    for (k = 0; k < dim+1; k++)
     if (nbc->vec[dof_y->vec[k]]==0) { 
      /* consider dof_y[k], try to compute x^neu_dh->vec[dof_y[k]] */

      /* consider first triangle el_info->el */
      el = el_info->el;
      (*get_loc_coords) (p_loc, el, phi_dh); 
    /*                p_loc are coordinates of the vertices of el */

      if(!proj_to_el(y_dh->vec[dof_y->vec[k]], 
		     (const REAL_D *)p_loc, &bary_coords)) {
	/* projection successful; */
	/*     save triangle vertex dofs and barycentric coords */
	for (l = 0; l < dim+1; l++) {
	  barcor[l]->vec[dof_y->vec[k]] = bary_coords[l];
	  gdof[l]->vec[dof_y->vec[k]] = dof_y->vec[l];
	}
	nbc->vec[dof_y->vec[k]] = dim+1;
	ctD++;
      }

/* RUNNING OVER NEIGHBOURNS DOES NOT YET WORK */
/*       else { /\* projection to el not successful, loop over neighbours *\/ */
/* 	i=0; /\* counter for neighbours *\/ */
/* 	do { */
/*           el = el_info->neigh[i]; */
/* 	  if (el) { */
/*             (*get_loc_coords) (el, phi_dh, p_loc); /\* p_loc are coordinates of the vertices of el *\/ */

/*             if (!proj_to_el (y_dh->vec[dof_y[k]], (const REAL_D *)p_loc, &bary_coords)) { */
/* 	      /\* successful, compute new vertex xneu *\/ */
/* 	      /\* need dofs of el for that *\/ */
/* 	      const DOF *dof_n = (*get_dof)(el, fe_space->admin, nil); */
/*            ... */
/* 	      i=N_NEIGH_MAX; */
/* 	      ctN++; */
/* 	    } */
/* 	  } /\* end if(el) *\/ */
/* 	  i++; */
/* 	} while (i<N_NEIGH_MAX); /\* at most N_NEIGH_MAX neighbours *\/ */
/*       } */
     }

    el_info = (EL_INFO *) traverse_next (stack, el_info);
  }
  free_traverse_stack (stack);

  /* second run */
  ctB=0;
  el_info = (EL_INFO *) traverse_first (stack, fe_space->mesh, -1, CALL_LEAF_EL);
  while (el_info) {
    (*get_loc_coords)(p_loc, el_info->el, phi_dh); /* p_loc local coords of el */

    // const DOF *dof_b = (*get_dof)(el_info->el, fe_space->admin, nil);
    const EL_DOF_VEC *dof_b = (*get_dof)(NULL, el_info->el, fe_space->admin, NULL);


    FOR_ALL_DOFS (fe_space->admin,
      if (nbc->vec[dof] == 0) { /* if not yet handled */
        if (!proj_to_el (y_dh->vec[dof], (const REAL_D *)p_loc, &bary_coords)) {
	  /* successful */
 	  for (l = 0; l < dim+1; l++) {
	    barcor[l]->vec[dof] = bary_coords[l];
	    gdof[l]->vec[dof] = dof_b->vec[l];
	  }
	  nbc->vec[dof] = dim+1;
	  ctB++;
	}
      }
    );
	
    el_info = (EL_INFO *) traverse_next (stack, el_info);
  }
  free_traverse_stack (stack);

  /* third part, handle points over edges */
  ctE = 0;
  FOR_ALL_DOFS (fe_space->admin,
    if (nbc->vec[dof] == 0) { /* dof not yet treated */

      /* find closest edge */
      d_min = 1.0e12;
      el_info = (EL_INFO *) traverse_first (stack, 
					    fe_space->mesh, -1, CALL_LEAF_EL);
      while (el_info) {
        (*get_loc_coords)(p_loc, el_info->el, phi_dh); /* p_loc local coords of el */
        //const DOF *dof_e = (*get_dof)(el_info->el, fe_space->admin, nil);
	const EL_DOF_VEC *dof_e = (*get_dof)(NULL, el_info->el, 
					      fe_space->admin, NULL);


        for (k = 0; k < dim+1; k++) { /* for every edge */
          /* v = y-p[k], e = (p[k+1]-p[k])/|%| */
	  for (l=0; l<DIM_OF_WORLD; l++) {
            v[l] = y_dh->vec[dof][l] - p_loc[k][l];
	    e[l] = p_loc[(k+1)%3][l] - p_loc[k][l];
	  }
	  len = NORM_DOW (e);
	  AX_DOW (1.0/len, e);

	  /* dist (y,g) = |v wedge e| where g is the line containing the edge e */
	  WEDGE_DOW (v, e, w);
	  d = NORM_DOW (w);

	  /* test if orthogonal projection to g is on e */
	  ht = SCP_DOW (v, e)/len;
	  if (ht < 0.0) /* y 'behind' p[k] */
	    d = DIST_DOW (y_dh->vec[dof], p_loc[k]);
	  if (ht > 1.0) /* y 'beyond' p[k+1] */
	    d = DIST_DOW (y_dh->vec[dof], p_loc[(k+1)%3]);

	  if (d < d_min) { /* potentially found right vertex */
	    d_min = d;
	    dof0 = dof_e->vec[k];
	    dof1 = dof_e->vec[(k+1)%3];
	    COPY_DOW (e, e_target);
	    COPY_DOW (v, v_target);
	    ht_target = ht;
	  }
	}
        el_info = (EL_INFO *) traverse_next (stack, el_info);
      }
      free_traverse_stack (stack);

      /* projection point of y on edge is pr = k e + p0 */
      /* with k = |(y-p0).e| and e = (p1-p0)/|%| = (phi_dh->vec[dof0]-phi_dh->vec[dof1])/|%|. */
      /* barycentric coords of pr are lam1 = k/|p1-p0| and lam0 = 1.0-lam1 */
      if (ht_target < 0.0) { /* y 'behind' p0 */
	lam1 = 0.0;
	lam0 = 1.0;
      }
      else {
        if (ht_target > 1.0) { /* y 'beyond' p1 */
	  lam1 = 1.0;
	  lam0 = 0.0;
        }
	else {
          lam1 = ht_target;
          lam0 = 1.0-lam1;
	}
      }

      barcor[0]->vec[dof] = lam0;
      barcor[1]->vec[dof] = lam1;
      gdof[0]->vec[dof] = dof0;
      gdof[1]->vec[dof] = dof1;
      nbc->vec[dof] = dim;
      ctE++;
    }
  );

  if (crd->project_info >= 2) {
    MSG ("# vertices on same triangle: %d\n", ctD);
    MSG ("# vertices on adjacent triangle: %d\n", ctN);
    MSG ("# vertices during second run: %d\n", ctB);
    MSG ("# vertices during third run: %d\n", ctE);
  }
  if (crd->project_info >= 4)
    print_dof_int_vec (nbc);

  return 0;
}

static void adapt_fields (DOF_REAL_D_VEC *x_dh, const struct reparam_cm_data *crd)
 {
  /* barcor and gdof need to be filled */
  /* set field^new[dof]=sum_l=0^nbc[dof] barcor[l][dof]*field^old[gdof[l][dof]] */
  /* for x_dh and all fields in crd->drvl and crd->drdvl */
  int n,l,k;

  /* new coords */
  FOR_ALL_DOFS (fe_space->admin,
    for (k = 0; k < DIM_OF_WORLD; k++) {
      xneu->vec[dof][k] = 0.0;
      for (l = 0; l < nbc->vec[dof]; l++)
	xneu->vec[dof][k] += barcor[l]->vec[dof] * x_dh->vec[gdof[l]->vec[dof]][k];
    }
  );
  dof_copy_d (xneu, x_dh);

  /* scalar fields on G_h */
  for (n = 0; n < crd->n_drvl; n++) {
    FOR_ALL_DOFS (fe_space->admin,
      hG1_h->vec[dof] = 0.0;
      for (l = 0; l < nbc->vec[dof]; l++)
	hG1_h->vec[dof] += barcor[l]->vec[dof] * crd->drvl[n]->vec[gdof[l]->vec[dof]];
    );
    dof_copy (hG1_h, crd->drvl[n]);
  }

  /* vector valued fields on G_h */
  for (n = 0; n < crd->n_drdvl; n++) {
    FOR_ALL_DOFS (fe_space->admin,
      for (k = 0; k < DIM_OF_WORLD; k++) {
        hG1_dh->vec[dof][k] = 0.0;
        for (l = 0; l < nbc->vec[dof]; l++)
	  hG1_dh->vec[dof][k] += barcor[l]->vec[dof] 
	    * crd->drdvl[n]->vec[gdof[l]->vec[dof]][k];
      }
    );
    dof_copy_d (hG1_dh, crd->drdvl[n]);
  }

  return;
}

/******************************************/
/* main routine                           */
/******************************************/

void reparam_cm (const DOF_REAL_D_VEC *y_dh, DOF_REAL_D_VEC *x_dh, 
		 const struct reparam_cm_data *crd) 
{
  /*   FUNCNAME("reparam_cm");     */

  /* initialisation of vectors and matrices */
  fe_space = x_dh->fe_space;
  init_global ();

  /* compute conformal map phi: Gamma_h -> S^2_h, store values in phi_dh */
  assemble (y_dh, x_dh, crd);
  compute_phi (y_dh, crd);

  /* compute barcor and gdof for projection of the y_dh-mesh to the mesh phi_dh(G_h) */
  project_nice_mesh (y_dh, crd);

  /* compute new vertex coords and other adapt fields on G_h */
  adapt_fields (x_dh, crd);

  /* free allocated space */
  terminate_global ();
}

