//                                                                        //
//   Author:    T.Warburton                                               //
//   Design:    T.Warburton && S.Sherwin                                  //
//   Date  :    12/4/96                                                   //
//                                                                        //
//   Copyright notice:  This code shall not be replicated or used without //
//                      the permission of the author.                     //
//                                                                        //
/**************************************************************************/

#include <stdarg.h>
#include <string.h>
#include <ctype.h>
#include <math.h>
#include <polylib.h>
#include "veclib.h"
#include "hotel.h"

#include <stdio.h>

using namespace polylib;

#define TOL_BLEND 1.0e-8

#define TANTOL 1e-10
/* new atan2 function to stop Nan on atan(0,0)*/
static double atan2_proof (double x, double y)
{
  if (fabs(x) + fabs(y) > TANTOL) return (atan2(x,y));
  else return (0.);
}
#define atan2 atan2_proof



typedef struct point    {  /* A 2-D point  */
  double  x,y;             /* coordinate   */
} Point;


double find_spiral_theta(Curve *curve, double x0, double y0, double z0);

void genCylinder(Curve *curve, double *x, double *y, double *z, int q){
  register int i;
  double   *f,theta,phi,cp,sp,ct,st;

  f = dvector(0,q-1);

  /* translate co-ordinates so that given point is origin */
  dsadd(q, -curve->info.cyl.xc, x, 1, x, 1);
  dsadd(q, -curve->info.cyl.yc, y, 1, y, 1);
  dsadd(q, -curve->info.cyl.zc, z, 1, z, 1);

  /* rotate co-ordinates so that cylinder axis is aligned with z axis */
  phi = atan2(curve->info.cyl.ay, curve->info.cyl.ax);
  cp  = cos(phi); sp = sin(phi);
  drot(1,&curve->info.cyl.ax,1,&curve->info.cyl.ay,1,cp,sp);
  theta = atan2(curve->info.cyl.ax,curve->info.cyl.az);
  ct    = cos(theta); st = sin(theta);

  drot(q,x,1,y,1,cp,sp);
  drot(q,z,1,x,1,ct,st);

  /* move co-ordinate out to surface value */
  for(i = 0; i < q; ++i){
    theta = atan2(y[i],x[i]);
    x[i]  = curve->info.cyl.radius*cos(theta);
    y[i]  = curve->info.cyl.radius*sin(theta);
  }

  /* rotate back */
  drot(q,z,1,x,1,ct,-st);
  drot(q,x,1,y,1,cp,-sp);

  /* translate co-ordinates back to original position */
  dsadd(q, curve->info.cyl.xc, x, 1, x, 1);
  dsadd(q, curve->info.cyl.yc, y, 1, y, 1);
  dsadd(q, curve->info.cyl.zc, z, 1, z, 1);

  free(f);
}

void genCone(Curve *curve, double *x, double *y, double *z, int q){
  register int i;
  double   *f,theta,phi,cp,sp,ct,st;

  f = dvector(0,q-1);

  /* translate co-ordinates so that apex is at origin */
  dsadd(q, -curve->info.cone.xc, x, 1, x, 1);
  dsadd(q, -curve->info.cone.yc, y, 1, y, 1);
  dsadd(q, -curve->info.cone.zc, z, 1, z, 1);

  /* rotate co-ordinates so that cone axis is aligned with z axis */
  phi   = atan2(curve->info.cone.ay, curve->info.cone.ax);
  cp = cos(phi); sp = sin(phi);
  drot(1,&curve->info.cone.ax,1,&curve->info.cone.ay,1,cp,sp);
  theta = atan2(curve->info.cone.ax,curve->info.cone.az);
  ct = cos(theta); st = sin(theta);

  drot(q,x,1,y,1,cp,sp);
  drot(q,z,1,x,1,ct,st);

  /* move co-ordinate out to surface value */
  for(i = 0; i < q; ++i){
    theta = atan2(y[i],x[i]);
    x[i]  = fabs(z[i])*curve->info.cone.alpha*cos(theta);
    y[i]  = fabs(z[i])*curve->info.cone.alpha*sin(theta);
  }

  /* rotate back */
  drot(q,z,1,x,1,ct,-st);
  drot(q,x,1,y,1,cp,-sp);

  /* translate co-ordinates back to original position */
  dsadd(q, curve->info.cone.xc, x, 1, x, 1);
  dsadd(q, curve->info.cone.yc, y, 1, y, 1);
  dsadd(q, curve->info.cone.zc, z, 1, z, 1);

  free(f);
}

void genSphere(Curve *curve, double *x, double *y, double *z, int q){
  register int i;
  double   *f,theta,phi;

  f = dvector(0,q-1);

  /* translate co-ordinates so that apex is at origin */
  dsadd(q, -curve->info.sph.xc, x, 1, x, 1);
  dsadd(q, -curve->info.sph.yc, y, 1, y, 1);
  dsadd(q, -curve->info.sph.zc, z, 1, z, 1);

  /* move co-ordinate out to surface value */
  for(i = 0; i < q; ++i){
    phi   = atan(z[i]/sqrt(x[i]*x[i]+y[i]*y[i]));
    theta = atan2(y[i],x[i]);
    z[i]  = curve->info.sph.radius*sin(phi);
    x[i]  = y[i]  = curve->info.sph.radius*cos(phi);
    x[i]  *= cos(theta);
    y[i]  *= sin(theta);
  }

  /* translate co-ordinates back to original position */
  dsadd(q, curve->info.sph.xc, x, 1, x, 1);
  dsadd(q, curve->info.sph.yc, y, 1, y, 1);
  dsadd(q, curve->info.sph.zc, z, 1, z, 1);

  free(f);
}

void genSheet(Curve *curve, double *x, double *y, double *z, int q){
  register int i;
  double   *f,theta,phi,cp,sp,ct,st,rtmp;

  f = dvector(0,q-1);

  /* translate co-ordinates so that given point is origin */
  dsadd(q, -curve->info.she.xc, x, 1, x, 1);
  dsadd(q, -curve->info.she.yc, y, 1, y, 1);
  dsadd(q, -curve->info.she.zc, z, 1, z, 1);

  /* rotate co-ordinates so that cylinder axis is aligned with z axis */
  phi   = atan2(curve->info.she.ay, curve->info.she.ax);
  cp = cos(phi); sp = sin(phi);
  drot(1,&curve->info.she.ax,1,&curve->info.she.ay,1,cp,sp);
  theta = atan2(curve->info.she.ax,curve->info.she.az);
  ct = cos(theta); st = sin(theta);

  drot(q,x,1,y,1,cp,sp);
  drot(q,z,1,x,1,ct,st);

  /* move co-ordinate out to surface value */
  for(i = 0; i < q; ++i){
    theta = z[i]*curve->info.she.twist+curve->info.she.zerotwistz;
    rtmp = sqrt(x[i]*x[i]+y[i]*y[i]);
    x[i]  = rtmp*cos(theta);
    y[i]  = rtmp*sin(theta);
  }
  /* rotate back */
  drot(q,z,1,x,1,ct,-st);
  drot(q,x,1,y,1,cp,-sp);

  /* translate co-ordinates back to original position */
  dsadd(q, curve->info.she.xc, x, 1, x, 1);
  dsadd(q, curve->info.she.yc, y, 1, y, 1);
  dsadd(q, curve->info.she.zc, z, 1, z, 1);

  free(f);
}

#ifdef OLDHELIX
void genSpiral(Curve *curve, double *x, double *y, double *z, int q){
  register int i;
  double   *f,theta,phi,cp,sp,ct,st,rtmp;

  f = dvector(0,q-1);

  /* translate co-ordinates so that given point is origin */
  dsadd(q, -curve->info.spi.xc, x, 1, x, 1);
  dsadd(q, -curve->info.spi.yc, y, 1, y, 1);
  dsadd(q, -curve->info.spi.zc, z, 1, z, 1);

  /* rotate co-ordinates so that cylinder axis is aligned with z axis */
  phi   = atan2(curve->info.spi.ay, curve->info.spi.ax);
  cp = cos(phi); sp = sin(phi);
  drot(1,&curve->info.spi.ax,1,&curve->info.spi.ay,1,cp,sp);
  theta = atan2(curve->info.spi.ax,curve->info.spi.az);
  ct = cos(theta); st = sin(theta);

  drot(q,x,1,y,1,cp,sp);
  drot(q,z,1,x,1,ct,st);

  /* move co-ordinate out to surface value */
  for(i = 0; i < q; ++i){
    theta = z[i]*curve->info.spi.twist+curve->info.spi.zerotwistz;
    rtmp = curve->info.spi.axialradius;
    x[i] -= rtmp*cos(theta);
    y[i] -= rtmp*sin(theta);
    phi   = atan2(y[i],x[i]);
    x[i]  = curve->info.spi.piperadius*cos(phi);
    y[i]  = curve->info.spi.piperadius*sin(phi);
    x[i] += rtmp*cos(theta);
    y[i] += rtmp*sin(theta);
  }
  /* rotate back */
  drot(q,z,1,x,1,ct,-st);
  drot(q,x,1,y,1,cp,-sp);

  /* translate co-ordinates back to original position */
  dsadd(q, curve->info.spi.xc, x, 1, x, 1);
  dsadd(q, curve->info.spi.yc, y, 1, y, 1);
  dsadd(q, curve->info.spi.zc, z, 1, z, 1);

  free(f);
}
#else


void genSpiral(Curve *curve, double *x, double *y, double *z, int q){
  register int i;
  double   *f,theta,theta0,phi,cp,sp,ct,st;
  double   axialrad,pitch,piperad;
  double   cx,cy,cz;

  f = dvector(0,q-1);

  /* translate co-ordinates so that given point is origin */
  dsadd(q, -curve->info.spi.xc, x, 1, x, 1);
  dsadd(q, -curve->info.spi.yc, y, 1, y, 1);
  dsadd(q, -curve->info.spi.zc, z, 1, z, 1);

  /* rotate co-ordinates so that cylinder axis is aligned with z axis */
  phi   = atan2(curve->info.spi.ay, curve->info.spi.ax);
  cp    = cos(phi);   sp = sin(phi);
  drot(1,&curve->info.spi.ax,1,&curve->info.spi.ay,1,cp,sp);
  theta = atan2(curve->info.spi.ax,curve->info.spi.az);
  ct    = cos(theta); st = sin(theta);

  drot(q,x,1,y,1,cp,sp);
  drot(q,z,1,x,1,ct,st);

  phi = 0.5*M_PI -
    atan(curve->info.spi.pitch/(2*M_PI*curve->info.spi.axialradius));

  piperad  = curve->info.spi.piperadius;
  axialrad = curve->info.spi.axialradius;
  pitch    = curve->info.spi.pitch;

  /* move co-ordinate out to surface value */
  for(i = 0; i < q; ++i){

    /* intially find theta0 assuming in first winding */
    theta0  = find_spiral_theta(curve,x[i],y[i],z[i]);
    cz      = 0.0;

    /* if theta0 gives a helix point which is outside pipe radius */
    /* assume that we are in the next winding and so re-calculate */
    while(fabs(z[i] - cz - pitch*theta0/(2*M_PI)) > piperad){
      cz     += pitch;
      theta0  = find_spiral_theta(curve,x[i],y[i],z[i]-cz);
    }

    cx  = axialrad*cos(theta0);
    cy  = axialrad*sin(theta0);
    cz += pitch*theta0/(2*M_PI);

    /* move section centre to origin */
    x[i] -= cx;
    y[i] -= cy;
    z[i] -= cz;

    /* rotate theta about z axis */
    drot(1,x+i,1,y+i,1,cos(theta0),sin(theta0));

    /* rotate -phi about x axis */
    drot(1,y+i,1,z+i,1,cos(-phi),sin(-phi));

    theta = atan2(y[i],x[i]);
    x[i]  = curve->info.spi.piperadius*cos(theta);
    y[i]  = curve->info.spi.piperadius*sin(theta);

    /* rotate phi about x axis */
    drot(1,y+i,1,z+i,1,cos(phi),sin(phi));

    /* rotate theta about z axis */
    drot(1,x+i,1,y+i,1,cos(-theta0),sin(-theta0));

    /* move section centre back */
    x[i] += cx;
    y[i] += cy;
    z[i] += cz;
  }

  /* rotate back */
  drot(q,z,1,x,1,ct,-st);
  drot(q,x,1,y,1,cp,-sp);

  /* translate co-ordinates back to original position */
  dsadd(q, curve->info.spi.xc, x, 1, x, 1);
  dsadd(q, curve->info.spi.yc, y, 1, y, 1);
  dsadd(q, curve->info.spi.zc, z, 1, z, 1);

  free(f);
}

#define TOLTHETA 1e-10
#define ITERTHETA 100
/* find the angle corresponding to the intersection of the helix and the plane
   containing the points x0,y0,z0 */

double find_spiral_theta(Curve *curve, double x0, double y0, double z0){
  register int     count = 0;
  double   dt;
  double   theta, theta0;
  double   pitch  = curve->info.spi.pitch;

  double   rad    = curve->info.spi.axialradius;
  double   A,B;


  theta = theta0 = atan2(y0,x0);
  /* correct for negative value of theta0 */
  theta0 += (fabs(z0 - pitch*theta0/(2*M_PI)) < rad)? 0:2*M_PI;

  theta = theta0;

  A = pitch*pitch/(4*M_PI*M_PI*rad);
  B = -x0*sin(theta0) + y0*cos(theta0) + z0*pitch/(2*M_PI*rad);

  dt = (-A*theta + B)/(rad+A);

  while((fabs(dt) > TOLTHETA)&&(count++ < ITERTHETA)){
    theta += dt;
    dt = (rad*sin(theta0 - theta) - A*theta + B)/(rad*cos(theta0-theta)+A);
  }
  if(count == ITERTHETA)
    fprintf(stderr,"Iterations failed to converge in spiral_theta\n");

  return theta;
}
#endif

void genTaurus(Curve *curve, double *x, double *y, double *z, int q){
  register int i;
  double   *f,theta,phi,cp,sp,ct,st,rtmp,xtmp,ytmp;

  f = dvector(0,q-1);

  /* translate co-ordinates so that given point is origin */
  dsadd(q, -curve->info.tau.xc, x, 1, x, 1);
  dsadd(q, -curve->info.tau.yc, y, 1, y, 1);
  dsadd(q, -curve->info.tau.zc, z, 1, z, 1);

  /* rotate co-ordinates so that tauinder axis is aligned with z axis */
  phi   = atan2(curve->info.tau.ay, curve->info.tau.ax);
  cp = cos(phi); sp = sin(phi);
  drot(1,&curve->info.tau.ax,1,&curve->info.tau.ay,1,cp,sp);
  theta = atan2(curve->info.tau.ax,curve->info.tau.az);
  ct = cos(theta); st = sin(theta);

  drot(q,x,1,y,1,cp,sp);
  drot(q,z,1,x,1,ct,st);

  /* move co-ordinate out to surface value */
  for(i = 0; i < q; ++i){
    theta = atan2(y[i],x[i]);
    rtmp = curve->info.tau.axialradius;
    xtmp = x[i];
    ytmp = y[i];
    x[i] = xtmp*cos(theta)+ytmp*sin(theta)-rtmp;
    y[i] = -xtmp*sin(theta)+ytmp*cos(theta);
    phi = atan2(z[i],x[i]);
    x[i] = curve->info.tau.piperadius*cos(phi)+rtmp;
    z[i] = curve->info.tau.piperadius*sin(phi);
    xtmp = x[i];
    ytmp = y[i];
    x[i]  = xtmp*cos(theta)-ytmp*sin(theta);
    y[i]  = xtmp*sin(theta)+ytmp*cos(theta);
  }

  /* rotate back */
  drot(q,z,1,x,1,ct,-st);
  drot(q,x,1,y,1,cp,-sp);

  /* translate co-ordinates back to original position */
  dsadd(q, curve->info.tau.xc, x, 1, x, 1);
  dsadd(q, curve->info.tau.yc, y, 1, y, 1);
  dsadd(q, curve->info.tau.zc, z, 1, z, 1);

  free(f);
}


void trans_coords(Coord *o, Coord *t, Coord *n, Coord *b,
      Coord *x, Coord *newx, int np, int dir){
  int i;


  // to (t,n,b)
  if(dir == 1){
    dcopy(np, x->x, 1, newx->x, 1);
    dcopy(np, x->y, 1, newx->y, 1);
    dcopy(np, x->z, 1, newx->z, 1);

    dsadd(np, -o->x[0], newx->x, 1, newx->x, 1);
    dsadd(np, -o->y[0], newx->y, 1, newx->y, 1);
    dsadd(np, -o->z[0], newx->z, 1, newx->z, 1);

    for(i=0;i<np;++i){
      newx->x[i] = t->x[0]*x->x[i] + t->y[0]*x->y[i] + t->z[0]*x->z[i];
      newx->y[i] = n->x[0]*x->x[i] + n->y[0]*x->y[i] + n->z[0]*x->z[i];
      newx->z[i] = b->x[0]*x->x[i] + b->y[0]*x->y[i] + b->z[0]*x->z[i];
    }

  }
  else{
    for(i=0;i<np;++i){
      newx->x[i] = t->x[0]*x->x[i] + n->x[0]*x->y[i] + b->x[0]*x->z[i];
      newx->y[i] = t->y[0]*x->x[i] + n->y[0]*x->y[i] + b->y[0]*x->z[i];
      newx->z[i] = t->z[0]*x->x[i] + n->z[0]*x->y[i] + b->z[0]*x->z[i];
    }

    /* translate co-ordinates back to original position */

    dsadd(np, o->x[0], newx->x, 1, newx->x, 1);
    dsadd(np, o->y[0], newx->y, 1, newx->y, 1);
    dsadd(np, o->z[0], newx->z, 1, newx->z, 1);
  }
}


#define c1       ( 0.29690)
#define c2       (-0.12600)
#define c3       (-0.35160)
#define c4       ( 0.28430)
#define c5       (-0.10360)

/* naca profile -- usage: naca t x  returns points on naca 00 aerofoil of
   thickness t at position x */

static double naca(double L, double x, double t){
  x = x/L;
  if(L==0.)
    return 0.;
  //  return 5.*t*L*(c1*sqrt(x)+ x*(c2 + x*(c3 + x*(c4 + c5*x))));
  return 5.*t*L*(c1*sqrt(x)+ x*(c2 + x*(c3 + x*(c4 + c5*x))));
}

#undef c1
#undef c2
#undef c3
#undef c4
#undef c5

void genNaca3d(Curve *curve, double *x, double *y, double *z, int q){
  register int i;
  Coord X,newX;
  double sg;

  X.x = x;
  X.y = y;
  X.z = z;

  newX.x = dvector(0, q-1);
  newX.y = dvector(0, q-1);
  newX.z = dvector(0, q-1);

  trans_coords(curve->info.nac3d.origin,
         curve->info.nac3d.axis,
         curve->info.nac3d.lead,
         curve->info.nac3d.locz,
         &X,&newX,q,1); // transform to aligned coord.s

  /* move co-ordinate out to surface value */
  for(i = 0; i < q; ++i){
    sg = (newX.z[i]<0) ? -1. : 1.;
    if(newX.x[i] < 0. || newX.x[i] > curve->info.nac3d.length)
      fprintf(stderr, "X: %lf\n", newX.x[i]);

    newX.z[i]  = sg*naca(curve->info.nac3d.length,
       newX.x[i],
       curve->info.nac3d.thickness);
  }
  trans_coords(curve->info.nac3d.origin,
         curve->info.nac3d.axis,
         curve->info.nac3d.lead,
         curve->info.nac3d.locz,
         &newX,&X,q,-1); // transform to aligned coord.s

  free(newX.x);
  free(newX.y);
  free(newX.z);
}



void Quad_Face_JacProj(Bndry *B){

  Element *E = B->elmt;
  int      face = B->face;
  const    int qa = E->qa, qb = E->qb;
  Coord    S;
  double   **da,**db,**dc,**dt;
  double   **D, *x, *y, *z, *xr, *xs, *yr, *ys, *zr, *zs;

  D = dmatrix(0,9,0,QGmax*QGmax-1);
  xr = D[0];  xs = D[1];
  yr = D[2];  ys = D[3];
  zr = D[4];  zs = D[5];

  S.x = D[0]; x = D[6];
  S.y = D[1]; y = D[7];
  S.z = D[2]; z = D[8];

  E->GetFaceCoord(face,&S);

  E->InterpToFace1(face,S.x,x);
  E->InterpToFace1(face,S.y,y);
  E->InterpToFace1(face,S.z,z);

  // db->da

  /* calculate derivatives */
  E->getD(&da,&dt,&db,&dt,&dc,&dt);

  /* calculate dx/dr */
  dgemm('T','N',qa,qb,qa,1.0,*da,qa,x,qa,0.0,xr,qa);

  /* calculate dx/ds */
  dgemm('N','N',qa,qb,qb,1.0,x,qa,*da,qb,0.0,xs,qa);

  /* calculate dy/dr */
  dgemm('T','N',qa,qb,qa,1.0,*da,qa,y,qa,0.0,yr,qa);

  /* calculate dy/ds */
  dgemm('N','N',qa,qb,qb,1.0,y,qa,*da,qb,0.0,ys,qa);

  /* calculate dz/dr */
  dgemm('T','N',qa,qb,qa,1.0,*da,qa,z,qa,0.0,zr,qa);

  /* calculate dz/ds */
  dgemm('N','N',qa,qb,qb,1.0,z,qa,*da,qb,0.0,zs,qa);

  /* x = yr*zs - zr*ys*/
  dvmul (qa*qb,yr,1,zs,1,x,1);
  dvvtvm(qa*qb,zr,1,ys,1,x,1,x,1);

  /* y = zr*xs - xr*zs*/
  dvmul (qa*qb,zr,1,xs,1,y,1);
  dvvtvm(qa*qb,xr,1,zs,1,y,1,y,1);

  /* z = xr*ys - xs*yr*/
  dvmul (qa*qb,xr,1,ys,1,z,1);
  dvvtvm(qa*qb,xs,1,yr,1,z,1,z,1);

  /* Surface Jacobea = sqrt(x^2 + y^2 + z^2) */

  dvmul (qa*qb,x,1,x,1,B->sjac.p,1);
  dvvtvp(qa*qb,y,1,y,1,B->sjac.p,1,B->sjac.p,1);
  dvvtvp(qa*qb,z,1,z,1,B->sjac.p,1,B->sjac.p,1);

  dvsqrt(qa*qb,B->sjac.p,1,B->sjac.p,1);

  free_dmatrix(D,0,0);
}
/* calculate the surface jacobian which is defined for as 2D surface in
   a 3D space as:

   Surface Jac  = sqrt(Nx^2 + Ny^2 + Nz^2)

   where [Nx,Ny,Nz] is the vector normal to the surface given by the
   cross product of the two tangent vectors in the r and s direction,
   i.e.

   Nx = y_r z_s - z_r y_s
   Ny = z_r x_s - x_r z_s
   Nz = x_r y_s - y_r x_s

   */

/* this function is defined with qa and qc to be compatible with the
prism and pyramid triangular faces */
void Tri_Face_JacProj(Bndry *B){
  register int i;
  Element *E = B->elmt;
  int      face = B->face;
  const    int qa = E->qa, qb = E->qb, qc = E->qc;
  Coord    S;
  double   **da,**db,**dc,**dt;
  double   **D, *x, *y, *z, *xr, *xs, *yr, *ys, *zr, *zs;
  Mode     *v = E->getbasis()->vert;

  D = dmatrix(0,9,0,QGmax*QGmax-1);
  xr = D[0];  xs = D[1];
  yr = D[2];  ys = D[3];
  zr = D[4];  zs = D[5];

  S.x = D[0]; x = D[6];
  S.y = D[1]; y = D[7];
  S.z = D[2]; z = D[8];

  E->GetFaceCoord(face,&S);

  E->InterpToFace1(face,S.x,x);
  E->InterpToFace1(face,S.y,y);
  E->InterpToFace1(face,S.z,z);

  /* calculate derivatives */
  E->getD(&da,&dt,&db,&dt,&dc,&dt);

  /* calculate dx/dr */
  dgemm('T','N',qa,qc,qa,1.0,*da,qa,x,qa,0.0,xr,qa);
  for(i = 0; i < qc; ++i)  dsmul(qa,1/v->c[i],xr+i*qa,1,xr+i*qa,1);

  /* calculate dx/ds */
  for(i = 0; i < qc; ++i) dvmul(qa,v[1].a,1,xr+i*qa,1,xs+i*qa,1);
  dgemm('N','N',qa,qc,qc,1.0,x,qa,*dc,qc,1.0,xs,qa);

  /* calculate dy/dr */
  dgemm('T','N',qa,qc,qa,1.0,*da,qa,y,qa,0.0,yr,qa);
  for(i = 0; i < qc; ++i)  dsmul(qa,1/v->c[i],yr+i*qa,1,yr+i*qa,1);

  /* calculate dy/ds */
  for(i = 0; i < qc; ++i) dvmul(qa,v[1].a,1,yr+i*qa,1,ys+i*qa,1);
  dgemm('N','N',qa,qc,qc,1.0,y,qa,*dc,qc,1.0,ys,qa);

  /* calculate dz/dr */
  dgemm('T','N',qa,qc,qa,1.0,*da,qa,z,qa,0.0,zr,qa);
  for(i = 0; i < qc; ++i)  dsmul(qa,1/v->c[i],zr+i*qa,1,zr+i*qa,1);

  /* calculate dz/ds */
  for(i = 0; i < qc; ++i) dvmul(qa,v[1].a,1,zr+i*qa,1,zs+i*qa,1);
  dgemm('N','N',qa,qc,qc,1.0,z,qa,*dc,qc,1.0,zs,qa);

  /* x = yr*zs - zr*ys*/
  dvmul (qa*qc,yr,1,zs,1,x,1);
  dvvtvm(qa*qc,zr,1,ys,1,x,1,x,1);

  /* y = zr*xs - xr*zs*/
  dvmul (qa*qc,zr,1,xs,1,y,1);
  dvvtvm(qa*qc,xr,1,zs,1,y,1,y,1);

  /* z = xr*ys - xs*yr*/
  dvmul (qa*qc,xr,1,ys,1,z,1);
  dvvtvm(qa*qc,xs,1,yr,1,z,1,z,1);

  /* Surface Jacobean = sqrt(x^2 + y^2 + z^2) */

  dvmul (qa*qc,x,1,x,1,B->sjac.p,1);
  dvvtvp(qa*qc,y,1,y,1,B->sjac.p,1,B->sjac.p,1);
  dvvtvp(qa*qc,z,1,z,1,B->sjac.p,1,B->sjac.p,1);

  dvsqrt(qa*qc,B->sjac.p,1,B->sjac.p,1);

  free_dmatrix(D,0,0);
}

#if 0
// only needed for explicit codes -- generate normals at Gauss points 'g'x'h'
void gen_face_normals(Element *E){
  int i;
  Bndry *Bc; // temporary bc

  Bc = (Bndry*) calloc(1,sizeof(Bndry));
  Bc->elmt = E;
  for(i=0;i<E->Nfaces;++i){
    Bc->face = i;
    E->Surface_geofac(Bc);
    if(Bc->sjac.p){
      // need to fix for variable order
      E->face[i].sjac.p = dvector(0, E->lmax*E->lmax-1);
      E->InterpToGaussFace(0, Bc->sjac.p, E->lmax, E->lmax, E->face[i].sjac.p);
      free(Bc->sjac.p);
      Bc->sjac.p = NULL;

      E->face[i].nx.p   = dvector(0, E->lmax*E->lmax-1);
      E->InterpToGaussFace(0,   Bc->nx.p, E->lmax, E->lmax,   E->face[i].nx.p);
      free(Bc->nx.p);
      Bc->nx.p = NULL;

      E->face[i].ny.p   = dvector(0, E->lmax*E->lmax-1);
      E->InterpToGaussFace(0,   Bc->ny.p, E->lmax, E->lmax,   E->face[i].ny.p);
      free(Bc->ny.p);
      Bc->ny.p = NULL;

      E->face[i].nz.p   = dvector(0, E->lmax*E->lmax-1);
      E->InterpToGaussFace(0,   Bc->nz.p, E->lmax, E->lmax,   E->face[i].nz.p);
      free(Bc->nz.p);
      Bc->nz.p = NULL;
    }
    else{
      E->face[i].sjac.d = Bc->sjac.d;
      E->face[i].nx.d   = Bc->nx.d;
      E->face[i].ny.d   = Bc->ny.d;
      E->face[i].nz.d   = Bc->nz.d;
    }
  }
}
#endif



void gen_ellipse(Element *E, Curve *cur, double *x, double *y){
  int i;
  double x0   = cur->info.ellipse.xo;
  double y0   = cur->info.ellipse.yo;
  double rmin = cur->info.ellipse.rmin;
  double rmaj = cur->info.ellipse.rmaj;

  Coord  X;
  X.x = dvector(0, QGmax-1);
  X.y = dvector(0, QGmax-1);

  double *xa = dvector(0, QGmax-1);
  double *ya = dvector(0, QGmax-1);

  E->straight_edge(&X, cur->face);
  E->InterpToFace1(cur->face, X.x, xa);
  E->InterpToFace1(cur->face, X.y, ya);

  double t0=0., t1=0.;

#if 1
  t0 = atan2((ya[0]-y0)*rmin, (xa[0]-x0)*rmaj);
  t1 = atan2((ya[E->qa-1]-y0)*rmin, (xa[E->qa-1]-x0)*rmaj);

  if(E->id == 162 || E->id == 387 || E->id == 445)
    fprintf(stderr, "id: %d t0: %lf t1: %lf\n", E->id+1,t0, t1);

#endif

  double t;

  double *z, *w;
  getzw(E->qa, &z, &w, 'a');

  if(t0 > 0 && t1 < 0){
    for(i=0;i<E->qa;++i){
      t = 0.5*(1-z[i])*t0 + 0.5*(1+z[i])*(t1+2.*M_PI);

      x[i] = x0 + rmaj*cos(t);
      y[i] = y0 + rmin*sin(t);
      if(E->id == 162 || E->id == 387 || E->id == 445)
  fprintf(stderr, "id: %d t: %lf x[%lf],y[%lf]\n", E->id+1, t, x[i] , y[i]);
    }
  }
  else if(t0 < 0 && t1 > 0){
    for(i=0;i<E->qa;++i){
      t = 0.5*(1-z[i])*(t0+2.*M_PI) + 0.5*(1+z[i])*t1;

      x[i] = x0 + rmaj*cos(t);
      y[i] = y0 + rmin*sin(t);
      if(E->id == 162 || E->id == 387 || E->id == 445)
  fprintf(stderr, "id: %d t: %lf x[%lf],y[%lf]\n", E->id+1, t, x[i] , y[i]);

    }
  }
  else{
    for(i=0;i<E->qa;++i){
      t = 0.5*(1-z[i])*t0 + 0.5*(1+z[i])*t1;

      x[i] = x0 + rmaj*cos(t);
      y[i] = y0 + rmin*sin(t);
      if(E->id == 162 || E->id == 387 || E->id == 445)
  fprintf(stderr, "id: %d t: %lf x[%lf],y[%lf]\n", E->id+1,t, x[i] , y[i]);

    }
  }


  free(xa);  free(ya);
  free(X.x); free(X.y);

  return;
}


void gen_sin(Element *E, Curve *cur, double *x, double *y){
  int i;
  double x0 = cur->info.sin.xo;
  double y0 = cur->info.sin.yo;
  double A  = cur->info.sin.amp;
  double lambda = cur->info.sin.wavelength;
  Coord  X;

  double *xa = dvector(0, QGmax-1);
  double *ya = dvector(0, QGmax-1);

  X.x = xa; X.y =ya;

  E->straight_edge(&X, cur->face);
  E->InterpToFace1(cur->face, X.x, x);
  for(i=0;i<E->qa;++i)
    y[i] = y0+A*sin(2.*M_PI*(x[i]-x0)/lambda);

  free(xa); free(ya);

  return;
}


/*******************************************************************/
void fillM(double **coord, double **d_dv, double **d_dw, double **d_dvdw, int i,int j, double **M);

void genFree(Curve *curve, double *x, double *y, double *z, char dir1, char dir2, int qa, int qb){
  //first 3 members of x,y,z contains coordinates of vertises
  int nvc,nwc,nvert,vert,i,j;
  double *xyz_vert, **vw;
  double *za,*zb,*w;        // quadrature points
  double xs,ys;          // local standard coordinate system
  double nu1,nu2;  // collapsed coordinate system
  double vq,wq;          // coordinates in parametric space
  double delta_1,delta_2;

  nvc = curve->info.free.nvc;
  nwc = curve->info.free.nwc;
  nvert = 3; // assume triangle

  if (nvert == 4){  //modify it later - nvert should be a parameter
    printf("projection of quadratic face on curved boundary is not implemented \n");
    exit(-1);
  }

  xyz_vert = dvector(0,nvert-1);
  vw = dmatrix(0,2,0,1);

  // get coordinates of vertices in parametric coordinate system
  for (vert = 0; vert < nvert; vert++){
    xyz_vert[0] = x[vert];
    xyz_vert[1] = y[vert];
    xyz_vert[2] = z[vert];
    curve->info.free.get_vw_safe(xyz_vert,vw[vert]);
  }
  /* in a case of periodic boundary  - fix elements that
     are crossed by periodic interface                    */

  if (curve->info.free.Vperiodic == 1){
    delta_1 = fabs(vw[0][0]-vw[1][0]);
    delta_2 = fabs(vw[1][0]-vw[2][0]);
    delta_1 = ( (delta_1 > delta_2) ? delta_1 : delta_2 );

    if (delta_1 > (0.5*nwc) ){
      for (i = 0; i < 3; i++){
        if ( vw[i][0] < (0.5*nwc))
          vw[i][0] = vw[i][0]+(-1.0+nwc);
      }
    }
  }

  if (curve->info.free.Wperiodic == 1){
    delta_1 = fabs(vw[0][1]-vw[1][1]);
    delta_2 = fabs(vw[1][1]-vw[2][1]);
    delta_1 = ( (delta_1 > delta_2) ? delta_1 : delta_2 );

    if (delta_1 > (0.5*nvc) ){
      for (i = 0; i < 3; i++){
        if ( vw[i][1] < (0.5*nvc))
          vw[i][1] = vw[i][1]+(-1.0+nvc);
      }
    }

  }

  //project all quadrature points on the face

  switch (dir1){
  case 'a':
    getzw(qa,&za,&w,'a');
    break;
  case 'b':
    getzw(qa,&za,&w,'b');
    break;
  case 'c':
    getzw(qa,&za,&w,'c');
    break;
  }
  switch (dir2){
  case 'a':
    getzw(qb,&zb,&w,'a');
    break;
  case 'b':
    getzw(qb,&zb,&w,'b');
    break;
  case 'c':
    getzw(qb,&zb,&w,'c');
    break;
  }

  for (j = 0; j < qb; j++){
    nu2 =  zb[j];

    for (i = 0; i < qa; i++){
      nu1 =  za[i];

      //transform from collapsed coordinate system to original xy
      //step 1
      //transform from collapsed coordinate system to standard element coord.
      xs = 0.5*(nu1+1.0)*(1.0-nu2)-1.0;
      ys = nu2;
      //step 2
      //transforms from standard element coordinate sysstem to original xy
      wq = vw[0][1]*(-ys-xs)+vw[1][1]*(1.0+xs)+vw[2][1]*(1.0+ys);
      wq *= 0.5;
      vq = vw[0][0]*(-ys-xs)+vw[1][0]*(1.0+xs)+vw[2][0]*(1.0+ys);
      vq *= 0.5;
      curve->info.free.interpolate2d(vq,wq,xyz_vert); // temporary use of array xyz_vert
      x[i+qa*j] = xyz_vert[0];
      y[i+qa*j] = xyz_vert[1];
      z[i+qa*j] = xyz_vert[2];
    }
  }

  free(xyz_vert);
  free_dmatrix(vw,0,0);

  return;
}

void C_Free::get_vw_safe(double *xyz,double *vw){

  int return_flag = 0, iter, MAX_iter_vw = 30;
  double Tol_vw = 1.0e-8;
  double delta_vw = 0.95;
  double error;
  int Npoints1d = 6;

  /* start from 2D Newton-Raphson */

  return_flag = get_vw(xyz,vw);
  if (return_flag == 0) return;


  /* 2D Newton-Raphson failed to converge */
  /* find v,w by 2D refining  */

  for (iter = 0; iter < MAX_iter_vw; ++iter){
    return_flag = get_vw(xyz,vw,delta_vw,Npoints1d,&error);

    if (return_flag == 0 && error < Tol_vw)  break;

    delta_vw *= 0.55;
  }
  //fprintf(stderr,"iter = %d error = %e\n",iter,error);
}



int C_Free::get_vw(double *xyz,double *vw){

  /* projects point(x,y,z) onto surface and
     returns vw coordinates and new x,y,z coordinate */

  double Tol = 1.0e-8;

  int i,j,Imin,Jmin;
  double *xyz_0,*xyz_1;
  double t;
  double dx,dy,dz,dr;
  double dnvc_m1,dnwc_m1;

  dnvc_m1 = (double) nvc - 1.0;
  dnwc_m1 = (double) nwc - 1.0;

  xyz_0 = dvector(0,2);
  xyz_1 = dvector(0,2);

   /* first guess */
  for (i = 0; i < 3; i++)
    xyz_0[i] = xyz[i];

  /* find closest point point from coordXYZ */
  t = 1000000.0;
  for (i = 0; i < nvc; i++){
    for (j = 0; j < nwc; j++){

      dx = xyz[0]-coordX[i][j];
      dy = xyz[1]-coordY[i][j];
      dz = xyz[2]-coordZ[i][j];

      dr = sqrt(dx*dx+dy*dy+dz*dz);

      if (dr < t){
        Imin = i;
        Jmin = j;
        t = dr;
        /* if closest point that defines the surface coinsides
        with xyz - exit */
        if (dr < Tol){
          vw[0] = 1.0*j;
          vw[1] = 1.0*i;
          free(xyz_0);
          free(xyz_1);
          return 0;
        }
      }
    }
  }

  if (Jmin == (nwc-1) )
    Jmin--;
  if (Imin == (nvc-1) )
    Imin--;

  /* Newton Rapson */
  int iter;
  double *vw_closest_pnt,*vw_0,*vw_n,**Jac,**invJac,*F_0;
  double *dXvw,*dYvw,*dZvw;
  double vw_first_closest_pnt[2];

  double error,error_save,error_save_closest_pnt;
  vw_closest_pnt = dvector(0,1);

  vw_0 = dvector(0,1);
  vw_n = dvector(0,1);

  dXvw = dvector(0,1);
  dYvw = dvector(0,1);
  dZvw = dvector(0,1);

  F_0 = dvector(0,1);

  Jac = dmatrix(0,1,0,1);
  invJac = dmatrix(0,1,0,1);

  /* first guesses */
  vw_0[0] = (double) Jmin; vw_0[1] = (double) Imin;
  vw[0] = vw_0[0];
  vw[1] = vw_0[1];
  vw_first_closest_pnt[0] = vw[0];
  vw_first_closest_pnt[1] = vw[1];

  interpolate_dvw_2d(vw_0[0],vw_0[1], xyz_1, dXvw, dYvw, dZvw);

  dx = xyz_1[0]-xyz_0[0];
  dy = xyz_1[1]-xyz_0[1];
  dz = xyz_1[2]-xyz_0[2];
  if (fabs(dx) < 0.1*Tol)
     dx = 0.1*Tol;
  if (fabs(dy) < 0.1*Tol)
     dy = 0.1*Tol;
  if (fabs(dz) < 0.1*Tol)
     dz = 0.1*Tol;

//  F_0[0] = dx*dx+dy*dy;
//  F_0[1] = dy*dy+dz*dz;

  for (i = 0; i < 2; i++)
    F_0[i] = (xyz_1[i]-xyz_0[i])*(xyz_1[i]-xyz_0[i])+(xyz_1[i+1]-xyz_0[i+1])*(xyz_1[i+1]-xyz_0[i+1]);

  error = sqrt(0.5*(F_0[0]*F_0[0]+F_0[1]*F_0[1]));

  if (error < Tol){
    vw[0] = vw_0[0];
    vw[1] = vw_0[1];
    error_save = error;
    goto clean_and_return;
   }

  error_save_closest_pnt = error;
  //printf("error_save_closest_pnt = %e \n",error);
  //printf("vw_0[0] = %f  vw_0[1] = %f \n",vw_0[0],vw_0[1]);
  //printf("xyz_1[0] = %f  xyz_1[1] = %f  xyz_1[2] = %f \n",xyz_1[0],xyz_1[1],xyz_1[2]);

  vw_closest_pnt[0] = vw_0[0];
  vw_closest_pnt[1] = vw_0[1];


  error_save = 1000000.0;

  for (iter = 0; iter < 20; iter++){
    //printf("iteration = %d \n",iter);

    /* fill Jacobian */
    dx = xyz_1[0]-xyz_0[0];
    dy = xyz_1[1]-xyz_0[1];
    dz = xyz_1[2]-xyz_0[2];
/*
    if (fabs(dx) < 0.1*Tol)
      dx = 0.1*Tol;
    if (fabs(dy) < 0.1*Tol)
      dy = 0.1*Tol;
    if (fabs(dz) < 0.1*Tol)
      dz = 0.1*Tol;
*/

#if 0
    Jac[0][0] = 2.0*dx*dXvw[0]+2.0*dy*dYvw[0];
    Jac[0][1] = 2.0*dx*dXvw[1]+2.0*dy*dYvw[1];
    Jac[1][0] = 2.0*dy*dYvw[0]+2.0*dz*dZvw[0];
    Jac[1][1] = 2.0*dy*dYvw[1]+2.0*dz*dZvw[1];

#else
    Jac[0][0] = 2.0*(xyz_1[0]-xyz_0[0])*dXvw[0]+2.0*(xyz_1[1]-xyz_0[1])*dYvw[0];
    Jac[0][1] = 2.0*(xyz_1[0]-xyz_0[0])*dXvw[1]+2.0*(xyz_1[1]-xyz_0[1])*dYvw[1];
    Jac[1][0] = 2.0*(xyz_1[1]-xyz_0[1])*dYvw[0]+2.0*(xyz_1[2]-xyz_0[2])*dZvw[0];
    Jac[1][1] = 2.0*(xyz_1[1]-xyz_0[1])*dYvw[1]+2.0*(xyz_1[2]-xyz_0[2])*dZvw[1];
#endif

    //printf("dXvw: %f %f \n ",dXvw[0],dXvw[1]);
    //printf("dYvw: %f %f \n ",dYvw[0],dYvw[1]);
    //printf("dZvw: %f %f \n ",dZvw[0],dZvw[1]);

    //printf("Jac[(0,0) (0,1)]: %f %f \n ",Jac[0][0],Jac[0][1]);
    //printf("Jac[(1,0) (1,1)]: %f %f \n ",Jac[1][0],Jac[1][1]);

     /* invert Jacobian */
#if 0
    error = (Jac[0][0]*Jac[1][1]-Jac[1][0]*Jac[0][1]); // temporary use of var. error
    if (fabs(error) < 1.0e-12)
      error = 1.0e-12;
    error = 1.0/error;
#else
    error = (Jac[0][0]*Jac[1][1]-Jac[1][0]*Jac[0][1]); // temporary use of var. error
    if (fabs(error) < 1.0e-12){
      error_save = 10000.0;
      //fprintf(stderr,"get_vw: det|Jac| is close to zero \n");
      vw_0[0] = vw[0] = vw_first_closest_pnt[0];
      vw_0[1] = vw[1] = vw_first_closest_pnt[1];
      goto  clean_and_return;
    }
    else
      error = 1.0/error; // temporary use of var. error
#endif

    invJac[0][0] = error*Jac[1][1];
    invJac[0][1] = -error*Jac[0][1];
    invJac[1][0] = -error*Jac[1][0];
    invJac[1][1] = error*Jac[0][0];

    vw_n[0] = invJac[0][0]*F_0[0]+invJac[0][1]*F_0[1];
    vw_n[1] = invJac[1][0]*F_0[0]+invJac[1][1]*F_0[1];

    //printf("vw_n[0] = %f  vw_n[1] = %f \n",vw_n[0],vw_n[1]);
    /* update previous result  */

    for (i = 0; i < 2; i++)
      vw_0[i] = vw_0[i]-vw_n[i];


    /* keep v  and w inside the domain */
    if (Vperiodic == 0){
      vw_0[0] = ( (vw_0[0] > 0.0) ? vw_0[0] : 0.0);         //MAX(vw[0],0.0);
      vw_0[0] = ( (vw_0[0] < dnwc_m1) ? vw_0[0] : dnwc_m1); //MIN(vw_0[0],dnvc_m1);
    }
    else{
      while (vw_0[0] < 0.0 || vw_0[0] > dnwc_m1){
        if (vw_0[0] < 0.0)
          vw_0[0] = dnwc_m1+vw_0[0];
        if (vw_0[0] > dnwc_m1)
          vw_0[0] = vw_0[0]- dnwc_m1;

       // printf("Vperiodic: vw_0[0] = %e \n",vw_0[0]);
      }
    }

    if (Wperiodic == 0){
      vw_0[1] = ( (vw_0[1] > 0.0) ? vw_0[1] : 0.0);         //MAX(vw[1],0.0);
      vw_0[1] = ( (vw_0[1] < dnvc_m1) ? vw_0[1] : dnvc_m1); //MIN(vw_0[1],dnwc_m1);
    }
    else{
      while (vw_0[1] < 0.0 || vw_0[1] > dnvc_m1){
        if (vw_0[1] < 0.0)
          vw_0[1] = dnvc_m1+vw_0[1];
        if (vw_0[1] > dnvc_m1)
          vw_0[1] = vw_0[1]- dnvc_m1;

        printf("Wperiodic: vw_0[1] = %e \n",vw_0[1]);
      }
    }

    //printf("vw_0[0] = %f vw_0[1] = %f \n",vw_0[0],vw_0[1]);

    interpolate_dvw_2d(vw_0[0],vw_0[1], xyz_1, dXvw, dYvw, dZvw);
    //printf("xyz_1[0] = %f  xyz_1[1] = %f  xyz_1[2] = %f \n",xyz_1[0],xyz_1[1],xyz_1[2]);

    for (i = 0; i < 2; i++)
      F_0[i] = (xyz_1[i]-xyz_0[i])*(xyz_1[i]-xyz_0[i])+(xyz_1[i+1]-xyz_0[i+1])*(xyz_1[i+1]-xyz_0[i+1]);


    /* estimate error */

    error = sqrt(0.5*(F_0[0]*F_0[0]+F_0[1]*F_0[1]));
   // printf("error = %e \n",error);

    /* save the best result */
    if (error_save > error){
      for (i = 0; i < 3; i++)
        xyz[i] = xyz_1[i];

      vw[0] = vw_0[0];
      vw[1] = vw_0[1];
      error_save = error;
    }

    if (error < Tol)
      break;
  }

  /* analyze error */
  if (error_save > 3.0e-1){
    if (error_save_closest_pnt < error_save){
      vw[0] = vw_closest_pnt[0];
      vw[1] = vw_closest_pnt[1];
      error_save = error_save_closest_pnt;
      interpolate_dvw_2d(vw[0],vw[1], xyz, dXvw, dYvw, dZvw);
    }
  }

  clean_and_return:

  //printf("error_save = %e vw = [%e %e]\n",error_save,vw[0],vw[1]);

  free(vw_closest_pnt);
  free(vw_0);
  free(vw_n);

  free(dXvw);
  free(dYvw);
  free(dZvw);

  free_dmatrix(Jac,0,0);
  free_dmatrix(invJac,0,0);

  if (error_save < 10.0*Tol)
    return 0;
  else{
    xyz[0] = xyz_0[0];
    xyz[1] = xyz_0[1];
    xyz[2] = xyz_0[2];
    return 1;
  }
}

int C_Free::get_vw(double *xyz,double *vw, double range, int Npoints, double *error){

  int i,j;
  double *v,*w,*xyz_temp,*xyz_save;
  double  error_ref,error_init,dx,dy,dz;
  double dnvc_m1,dnwc_m1;

  dnvc_m1 = (double) nvc - 1.0;
  dnwc_m1 = (double) nwc - 1.0;

  v = new double[Npoints*2];
  w = v+Npoints;
  xyz_temp = new double[6];
  xyz_save = xyz_temp+3;


  v[0] = vw[0]-range;
  v[Npoints-1] = vw[0]+range;

  if ( Vperiodic == 0){
    v[0] = ( (v[0] > 0.0) ? v[0] : 0.0);         //MAX(vw[0],0.0);
    v[0] = ( (v[0] < dnwc_m1) ? v[0] : dnwc_m1); //MIN(vw_0[0],dnwc_m1);
    v[Npoints-1] = ( (v[Npoints-1] > 0.0) ? v[Npoints-1] : 0.0);         //MAX(vw[0],0.0);
    v[Npoints-1] = ( (v[Npoints-1] < dnwc_m1) ? v[Npoints-1] : dnwc_m1); //MIN(vw_0[0],dnwc_m1);
  }

  w[0] = vw[1]-range;
  w[Npoints-1] = vw[1]+range;
  if (Wperiodic == 0){
    w[0] = ( (w[0] > 0.0) ? w[0] : 0.0);         //MAX(vw[0],0.0);
    w[0] = ( (w[0] < dnvc_m1) ? w[0] : dnvc_m1); //MIN(vw_0[0],dnvc_m1);
    w[Npoints-1] = ( (w[Npoints-1] > 0.0) ? w[Npoints-1] : 0.0);         //MAX(vw[0],0.0);
    w[Npoints-1] = ( (w[Npoints-1] < dnvc_m1) ? w[Npoints-1] : dnvc_m1); //MIN(vw_0[0],dnvc_m1);
  }


  dx = (v[Npoints-1]-v[0])/(-1.0+Npoints);
  for (i = 1; i < Npoints; ++i)
    v[i] = v[0]+dx*i;


  dx = (w[Npoints-1]-w[0])/(-1.0+Npoints);
  for (i = 1; i < Npoints; ++i)
    w[i] = w[0]+dx*i;



  //fprintf(stderr,"vw = [%f %f], range v = [%f %f],range w = [%f %f]\n", vw[0],vw[1],v[0],v[Npoints-1],w[0],w[Npoints-1]);

  interpolate2d(vw[0],vw[1],xyz_temp);

  dx = xyz_temp[0]-xyz[0];
  dy = xyz_temp[1]-xyz[1];
  dz = xyz_temp[2]-xyz[2];

  error_ref = sqrt(dx*dx+dy*dy+dz*dz);
  error_init = error_ref;

  for (i = 0; i < Npoints; ++i){
    for (j = 0; j < Npoints; ++j){
      interpolate2d(v[i],w[j],xyz_temp);
      dx = xyz_temp[0]-xyz[0];
      dy = xyz_temp[1]-xyz[1];
      dz = xyz_temp[2]-xyz[2];
      error[0] = sqrt(dx*dx+dy*dy+dz*dz);
      //fprintf(stderr,"v = %f w = %f error = %f \n",v[i],w[j],error[0]);
      if (error[0] < error_ref){
        vw[0] = v[i];
        vw[1] = w[j];
        error_ref=error[0];
      }
    }
  }
  delete[] v;
  delete[] xyz_temp;

  //fprintf(stderr,"error_init = %e, error_refin = %e \n",error_init,error[0]);

  if (error[0] < error_init)
    return 0;
  else
    return 1;
}



void C_Free::interpolate_dvw_2d(double v1, double w1, double *xyz, double *dXvw, double *dYvw, double *dZvw){

/*  r(v,w) = [1 v v^2 v^3]*C*M*CT*[1 w w^2 w^3]'  */

  int i,j;
  double *ANS1, *ANS2, **Mtemp;

  ANS1 = dvector(0,3);
  ANS2 = dvector(0,3);
  Mtemp = dmatrix(0,3,0,3);

  /* global coordinates */
  vg = v1;
  wg = w1;

  /* cell number */
  i = (int) wg;
  j = (int) vg;

  if (i >= (nvc-1) )
    i--;
  if (j >= (nwc-1) )
    j--;

  /* if CMCT was computed for different cell than recompute it */
  /* else - do nothing - use existing CMCT */
  //if (i==Icell && j==Jcell)
  //  i=i;
  //else{
  if (i != Icell || j != Jcell){
    fillM(coordX,dx_dv,dx_dw,dx_dvdw,i,j,M);
    dgemm('T', 'T', 4, 4, 4, 1.0, *C,     4, *M,  4, 0.0, *Mtemp, 4);
    dgemm('N', 'T', 4, 4, 4, 1.0, *CT, 4, *Mtemp, 4, 0.0, *CMCTx, 4);
    ;
    fillM(coordY,dy_dv,dy_dw,dy_dvdw,i,j,M);
    dgemm('T', 'T', 4, 4, 4, 1.0, *C,     4, *M,  4, 0.0, *Mtemp, 4);
    dgemm('N', 'T', 4, 4, 4, 1.0, *CT, 4, *Mtemp, 4, 0.0, *CMCTy, 4);

    fillM(coordZ,dz_dv,dz_dw,dz_dvdw,i,j,M);
    dgemm('T', 'T', 4, 4, 4, 1.0, *C,     4, *M,  4, 0.0, *Mtemp, 4);
    dgemm('N', 'T', 4, 4, 4, 1.0, *CT, 4, *Mtemp, 4, 0.0, *CMCTz, 4);
    Icell = i;
    Jcell = j;
  }

  /*  local coordinates */
  vl = vg - ( (double) j);
  wl = wg - ( (double) i);

  V[0] = 1.0; V[1] = vl; V[2] = vl*vl; V[3] = vl*vl*vl;
  W[0] = 1.0; W[1] = wl; W[2] = wl*wl; W[3] = wl*wl*wl;

  /* compute coordinates x,y,z */
  dgemv('T',4,4,1.0,*CMCTx,4,W,1,0.0,ANS1,1);
  xyz[0] =  ddot(4,V,1,ANS1,1);

  dgemv('T',4,4,1.0,*CMCTy,4,W,1,0.0,ANS1,1);
  xyz[1] =  ddot(4,V,1,ANS1,1);

  dgemv('T',4,4,1.0,*CMCTz,4,W,1,0.0,ANS1,1);
  xyz[2] =  ddot(4,V,1,ANS1,1);

  /* compute partial derivatives  */
   /* d / dv */
   V[0] = 0.0; V[1] = 1.0; V[2] = 2.0*vl; V[3] = 3.0*vl*vl;

   dgemv('T',4,4,1.0,*CMCTx,4,W,1,0.0,ANS1,1);
   dXvw[0] =  ddot(4,V,1,ANS1,1);

   dgemv('T',4,4,1.0,*CMCTy,4,W,1,0.0,ANS1,1);
   dYvw[0] =  ddot(4,V,1,ANS1,1);

   dgemv('T',4,4,1.0,*CMCTz,4,W,1,0.0,ANS1,1);
   dZvw[0] =  ddot(4,V,1,ANS1,1);

    /* d / dw */
   V[0] = 1.0; V[1] = vl; V[2] = vl*vl; V[3] = vl*vl*vl;
   W[0] = 0.0; W[1] = 1.0; W[2] = 2.0*wl; W[3] = 3.0*wl*wl;

   dgemv('T',4,4,1.0,*CMCTx,4,W,1,0.0,ANS1,1);
   dXvw[1] =  ddot(4,V,1,ANS1,1);

   dgemv('T',4,4,1.0,*CMCTy,4,W,1,0.0,ANS1,1);
   dYvw[1] =  ddot(4,V,1,ANS1,1);

   dgemv('T',4,4,1.0,*CMCTz,4,W,1,0.0,ANS1,1);
   dZvw[1] =  ddot(4,V,1,ANS1,1);

   free(ANS1);
   free(ANS2);
}

void C_Free::interpolate2d(double v1, double w1, double *xyz){

/*  r(v,w) = [1 v v^2 v^3]*C*M*CT*[1 w w^2 w^3]'  */

  int i,j;
  double *ANS1, *ANS2;
  double  dnvc_m1 = -1.0+nvc;
  double  dnwc_m1 = -1.0+nwc;

  ANS1 = dvector(0,3);
  ANS2 = dvector(0,3);

  /* global coordinates */
  vg = v1;
  wg = w1;

  /* keep v  and w inside the domain */
    if (Vperiodic == 0){
      vg = ( (vg > 0.0) ? vg : 0.0);         //MAX(vw[0],0.0);
      vg = ( (vg < dnwc_m1) ? vg : dnwc_m1); //MIN(vw_0[0],dnvc_m1);
    }
    else{
      if (vg < 0.0)
        vg = dnwc_m1+vg;
      if (vg > dnwc_m1)
        vg = vg-dnwc_m1;
    }

    if (Wperiodic == 0){
      wg = ( (wg > 0.0) ? wg : 0.0);         //MAX(vw[1],0.0);
      wg = ( (wg < dnvc_m1) ? wg : dnvc_m1); //MIN(vw_0[1],dnwc_m1);
    }
    else{
      if (wg < 0.0)
        wg = dnvc_m1+wg;
      if (wg > dnvc_m1)
        wg = wg- dnvc_m1;
    }


  /* cell number */
  i = (int) wg;
  j = (int) vg;

 if (i >= (nvc-1) )
    i--;
  if (j >= (nwc-1) )
    j--;

  /*  local coordinates */
  vl = vg - ( (double) j);
  wl = wg - ( (double) i);

  V[0] = 1.0; V[1] = vl; V[2] = vl*vl; V[3] = vl*vl*vl;
  W[0] = 1.0; W[1] = wl; W[2] = wl*wl; W[3] = wl*wl*wl;

  fillM(coordX,dx_dv,dx_dw,dx_dvdw,i,j,M);
  dgemv('T',4,4,1.0,*CT,4,W,1,0.0,ANS1,1);
  dgemv('T',4,4,1.0,*M,4,ANS1,1,0.0,ANS2,1);
  dgemv('T',4,4,1.0,*C,4,ANS2,1,0.0,ANS1,1);
  xyz[0] = ddot(4,V,1,ANS1,1);

  fillM(coordY,dy_dv,dy_dw,dy_dvdw,i,j,M);

  dgemv('T',4,4,1.0,*CT,4,W,1,0.0,ANS1,1);
  dgemv('T',4,4,1.0,*M,4,ANS1,1,0.0,ANS2,1);
  dgemv('T',4,4,1.0,*C,4,ANS2,1,0.0,ANS1,1);
  xyz[1] = ddot(4,V,1,ANS1,1);


  fillM(coordZ,dz_dv,dz_dw,dz_dvdw,i,j,M);
  dgemv('T',4,4,1.0,*CT,4,W,1,0.0,ANS1,1);
  dgemv('T',4,4,1.0,*M,4,ANS1,1,0.0,ANS2,1);
  dgemv('T',4,4,1.0,*C,4,ANS2,1,0.0,ANS1,1);
  xyz[2] = ddot(4,V,1,ANS1,1);

  free(ANS1);
  free(ANS2);

}

void fillM(double **coord, double **d_dv, double **d_dw, double **d_dvdw, int i,int j, double **M){
  M[0][0] = coord[i][j];   M[0][1] = coord[i+1][j];   M[0][2] = d_dw[i][j];     M[0][3] = d_dw[i+1][j];
  M[1][0] = coord[i][j+1]; M[1][1] = coord[i+1][j+1]; M[1][2] = d_dw[i][j+1];   M[1][3] = d_dw[i+1][j+1];
  M[2][0] = d_dv[i][j];    M[2][1] = d_dv[i+1][j];    M[2][2] = d_dvdw[i][j];   M[2][3] = d_dvdw[i+1][j];
  M[3][0] = d_dv[i][j+1],  M[3][1] = d_dv[i+1][j+1];  M[3][2] = d_dvdw[i][j+1]; M[3][3] = d_dvdw[i+1][j+1];
}


/*******************************************************************/

double crossprodmag(double *a, double *b)
{
  double mag, c[3];
  c[0] = a[1]*b[2] - a[2]*b[1];
  c[1] = a[2]*b[0] - a[0]*b[2];
  c[2] = a[0]*b[1] - a[1]*b[0];
  mag = sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2]);
  return mag;
}

void crossprod(double *a, double *b, double *c)
{
  c[0] = a[1]*b[2] - a[2]*b[1];
  c[1] = a[2]*b[0] - a[0]*b[2];
  c[2] = a[0]*b[1] - a[1]*b[0];
  return;
}

void unitcrossprod(double *a, double *b, double *c){
  double inv_mag;

  c[0] = a[1]*b[2] - a[2]*b[1];
  c[1] = a[2]*b[0] - a[0]*b[2];
  c[2] = a[0]*b[1] - a[1]*b[0];

  inv_mag = 1.0/sqrt(c[0]*c[0] + c[1]*c[1] + c[2]*c[2]);

  c[0] = c[0]*inv_mag;
  c[1] = c[1]*inv_mag;
  c[2] = c[2]*inv_mag;
  return;
}

double blend(double r){
 return r*r;
}

void superblend(double *ri, double **Q, double *P, double *r_blend ){

   int i;
   double rn, v0_Qi_P_s, v1_Qi_P_s, v2_Qi_P_s;
   rn = 1.0-TOL_BLEND;
   r_blend[0] = r_blend[1] = r_blend[2] = 0.0;


   for ( i = 0; i < 3; ++i)
     if ( ri[i] > rn ){
       r_blend[i] = 1.0;
       return;
     }

   v0_Qi_P_s = (Q[0][0]-P[0])*(Q[0][0]-P[0]) +
               (Q[0][1]-P[1])*(Q[0][1]-P[1]) +
               (Q[0][2]-P[2])*(Q[0][2]-P[2]);

   v1_Qi_P_s = (Q[1][0]-P[0])*(Q[1][0]-P[0]) +
               (Q[1][1]-P[1])*(Q[1][1]-P[1]) +
               (Q[1][2]-P[2])*(Q[1][2]-P[2]);

   v2_Qi_P_s = (Q[2][0]-P[0])*(Q[2][0]-P[0]) +
               (Q[2][1]-P[1])*(Q[2][1]-P[1]) +
               (Q[2][2]-P[2])*(Q[2][2]-P[2]);


   if (ri[0] > TOL_BLEND)
     r_blend[0] = ri[0]*ri[0] * ( ri[2]*ri[2]* v2_Qi_P_s/(v2_Qi_P_s+v0_Qi_P_s) + ri[1]*ri[1]*v1_Qi_P_s/(v1_Qi_P_s+v0_Qi_P_s) );
   if (ri[1] > TOL_BLEND)
     r_blend[1] = ri[1]*ri[1] * ( ri[0]*ri[0]* v0_Qi_P_s/(v0_Qi_P_s+v1_Qi_P_s) + ri[2]*ri[2]*v2_Qi_P_s/(v2_Qi_P_s+v1_Qi_P_s) );
   if (ri[2] > TOL_BLEND)
     r_blend[2] = ri[2]*ri[2] * ( ri[1]*ri[1]* v1_Qi_P_s/(v1_Qi_P_s+v2_Qi_P_s) + ri[0]*ri[0]*v0_Qi_P_s/(v0_Qi_P_s+v2_Qi_P_s) );


   rn = 1.0/sqrt(r_blend[0] + r_blend[1] + r_blend[2]);
   r_blend[0] *= rn;
   r_blend[1] *= rn;
   r_blend[2] *= rn;
}


void genRecon(Curve *curve, double *va, double *vb, double *vc, double *x, double *y, double *z, int q){

  int i;
  double ra, rb, rc, *vna, *vnb, *vnc, ta[3], tb[3], a[3],b[3],c[3];
  double A, ma, mb, mc, blenda, blendb, blendc, totalblend, inv_totalblend;
  double conta, contb, contc, tha, thb, thc;
  double Nx, Ny, Nz , ka[3], kb[3], kc[3];
  double qa[3], qb[3], qc[3];

  double *P,*r_blend,*ri;
  double **Q = dmatrix(0,2,0,2);
  double MAXANGLE = dparam("DMAXANGLESUR");

  P = dvector(0,8);
  r_blend = P+3;
  ri      = P+6;

  double N[3];

  vna = curve->info.recon.vn0;
  vnb = curve->info.recon.vn1;
  vnc = curve->info.recon.vn2;

  ta[0] = vb[0] - va[0];
  ta[1] = vb[1] - va[1];
  ta[2] = vb[2] - va[2];

  tb[0] = vc[0] - va[0];
  tb[1] = vc[1] - va[1];
  tb[2] = vc[2] - va[2];

  unitcrossprod(tb, ta, N);

  tha = fabs(asin(crossprodmag(N, vna)));
  thb = fabs(asin(crossprodmag(N, vnb)));
  thc = fabs(asin(crossprodmag(N, vnc)));

  if( (tha>MAXANGLE) || (thb >MAXANGLE) || (thc>MAXANGLE) )
  {
      //ROOTONLY fprintf(stderr, "This may cause problem Angle(degree) between face normal and averaged normals %lf %lf %lf \n",
      //tha*180.0/M_PI, thb*180/M_PI, thc*180.0/M_PI);
   return;
  }

  A = crossprodmag(ta, tb);

  for(i=0;i<q; i++)
  {
   a[0] = x[i]-va[0];
   a[1] = y[i]-va[1];
   a[2] = z[i]-va[2];

   b[0] = x[i]-vb[0];
   b[1] = y[i]-vb[1];
   b[2] = z[i]-vb[2];

   c[0] = x[i]-vc[0];
   c[1] = y[i]-vc[1];
   c[2] = z[i]-vc[2];


    ra = crossprodmag(b,c);
    ra = ra/A;
    rb = crossprodmag(c,a);
    rb = rb/A;
    rc = crossprodmag(a,b);
    rc = rc/A;

    Nx = vna[0]*ra + vnb[0]*rb + vnc[0]*rc;
    Ny = vna[1]*ra + vnb[1]*rb + vnc[1]*rc;
    Nz = vna[2]*ra + vnb[2]*rb + vnc[2]*rc;

    conta = (va[0]-x[i])*Nx + (va[1]-y[i])*Ny + (va[2]-z[i])*Nz;
    contb = (vb[0]-x[i])*Nx + (vb[1]-y[i])*Ny + (vb[2]-z[i])*Nz;
    contc = (vc[0]-x[i])*Nx + (vc[1]-y[i])*Ny + (vc[2]-z[i])*Nz;

    ka[0] = x[i] + conta*Nx;
    ka[1] = y[i] + conta*Ny;
    ka[2] = z[i] + conta*Nz;
    kb[0] = x[i] + contb*Nx;
    kb[1] = y[i] + contb*Ny;
    kb[2] = z[i] + contb*Nz;
    kc[0] = x[i] + contc*Nx;
    kc[1] = y[i] + contc*Ny;
    kc[2] = z[i] + contc*Nz;

    conta = (va[0]-ka[0])*vna[0] + (va[1]-ka[1])*vna[1] + (va[2]-ka[2])*vna[2];
    contb = (vb[0]-kb[0])*vnb[0] + (vb[1]-kb[1])*vnb[1] + (vb[2]-kb[2])*vnb[2];
    contc = (vc[0]-kc[0])*vnc[0] + (vc[1]-kc[1])*vnc[1] + (vc[2]-kc[2])*vnc[2];

    ma = conta/(1. + Nx*vna[0] +  Ny*vna[1]  + Nz*vna[2]) ;
    mb = contb/(1. + Nx*vnb[0] +  Ny*vnb[1]  + Nz*vnb[2]) ;
    mc = contc/(1. + Nx*vnc[0] +  Ny*vnc[1]  + Nz*vnc[2]) ;

    qa[0] = ka[0] + ma*Nx;
    qa[1] = ka[1] + ma*Ny;
    qa[2] = ka[2] + ma*Nz;

    qb[0] = kb[0] + mb*Nx;
    qb[1] = kb[1] + mb*Ny;
    qb[2] = kb[2] + mb*Nz;

    qc[0] = kc[0] + mc*Nx;
    qc[1] = kc[1] + mc*Ny;
    qc[2] = kc[2] + mc*Nz;
    blenda = blend(ra);
    blendb = blend(rb);
    blendc = blend(rc);
    totalblend = blenda + blendb + blendc;
    inv_totalblend = 1.0/totalblend;

#if 0
    ri[0] = ra;
    ri[1] = rb;
    ri[2] = rc;
    P[0] = x[i];
    P[1] = y[i];
    P[2] = z[i];

    conta = (va[0]-x[i])*Nx + (va[1]-y[i])*Ny + (va[2]-z[i])*Nz;
    contb = (vb[0]-x[i])*Nx + (vb[1]-y[i])*Ny + (vb[2]-z[i])*Nz;
    contc = (vc[0]-x[i])*Nx + (vc[1]-y[i])*Ny + (vc[2]-z[i])*Nz;


    Q[0][0] = va[0]-conta*Nx;
    Q[0][1] = va[1]-conta*Ny;
    Q[0][2] = va[2]-conta*Nz;

    Q[1][0] = vb[0]-contb*Nx;
    Q[1][1] = vb[1]-contb*Ny;
    Q[1][2] = vb[2]-contb*Nz;

    Q[2][0] = vc[0]-contc*Nx;
    Q[2][1] = vc[1]-contc*Ny;
    Q[2][2] = vc[2]-contc*Nz;

    superblend(ri, Q, P, r_blend);

    x[i] = r_blend[0]*qa[0] + r_blend[1]*qb[0] + r_blend[2]*qc[0];
    y[i] = r_blend[0]*qa[1] + r_blend[1]*qb[1] + r_blend[2]*qc[1];
    z[i] = r_blend[0]*qa[2] + r_blend[1]*qb[2] + r_blend[2]*qc[2];

//    x[i] = r_blend[0]*Q[0][0] + r_blend[1]*Q[1][0] + r_blend[2]*Q[2][0];
//    y[i] = r_blend[0]*Q[0][1] + r_blend[1]*Q[1][1] + r_blend[2]*Q[2][1];
//    z[i] = r_blend[0]*Q[0][2] + r_blend[1]*Q[1][2] + r_blend[2]*Q[2][2];

#else
    x[i] = (blenda*qa[0] + blendb*qb[0] + blendc*qc[0])*inv_totalblend;
    y[i] = (blenda*qa[1] + blendb*qb[1] + blendc*qc[1])*inv_totalblend;
    z[i] = (blenda*qa[2] + blendb*qb[2] + blendc*qc[2])*inv_totalblend;
#endif

 }
  free(P);
  free_dmatrix(Q,0,0);
  return;
}




/*******************************************************************/




#define c1       ( 0.29690)
#define c2       (-0.12600)
#define c3       (-0.35160)
#define c4       ( 0.28430)
#define c5       (-0.10360)

/* naca profile -- usage: naca t x  returns points on naca 00 aerofoil of
   thickness t at position x

   to compile

   cc -o naca naca.c -lm
   */

double Tri_naca(double L, double x, double t){
  x = x/L;
  if(L==0.)
    return 0.;
  //  return 5.*t*L*(c1*sqrt(x)+ x*(c2 + x*(c3 + x*(c4 + c5*x))));
  return 5.*t*L*(c1*sqrt(x)+ x*(c2 + x*(c3 + x*(c4 + c5*x))));
}

#undef c1
#undef c2
#undef c3
#undef c4
#undef c5

void Tri_genNaca(Element *E, Curve *curve, double *x, double *y){
  int   i;
  Coord X;
  X.x = dvector(0, QGmax-1);
  X.y = dvector(0, QGmax-1);

  E->GetFaceCoord(curve->face, &X);

  dcopy(E->qa, X.x, 1, x, 1);
  for(i=0;i<E->qa;++i){
    y[i] = Tri_naca(curve->info.nac2d.length,
    X.x[i]-curve->info.nac2d.xo,
    curve->info.nac2d.thickness);
    if(X.y[i] < curve->info.nac2d.yo)
      y[i] =  curve->info.nac2d.yo-y[i];
    else
      y[i] =  curve->info.nac2d.yo+y[i];
  }

  free(X.x);
  free(X.y);
}


#define distance(p1,p2) (sqrt((p2.x-p1.x)*(p2.x-p1.x)+(p2.y-p1.y)*(p2.y-p1.y)))

#define ITERNACA 1000
#define TOLNACA  1e-5
#define c1       ( 0.29690)
#define c2       (-0.12600)
#define c3       (-0.35160)
#define c4       ( 0.28430)
// #define c5       (-0.10150)
#define c5       (-0.10360)
/*all that follows is to set up a spline fitting routine from a data file*/

typedef struct geomf  {    /* Curve defined in a file */
  int           npts  ;    /* number of points        */
  int           pos   ;    /* last confirmed position */
  char         *name  ;    /* File/curve name         */
  double       *x, *y ;    /* coordinates             */
  double       *sx,*sy;    /* spline coefficients     */
  double       *arclen;    /* arclen along the curve  */
  struct geomf *next  ;    /* link to the next        */
} Geometry;

typedef struct vector {    /* A 2-D vector */
  double     x, y     ;    /* components   */
  double     length   ;    /* length       */
} Vector;

#define _MAX_NC         1024   /* Points describing a curved side   */
static int    closest    (Point p, Geometry *g);
static void   bracket    (double s[], double f[], Geometry *g, Point a,
        Vector ap);
static Vector setVector  (Point p1, Point p2);

static double searchGeom (Point a, Point p, Geometry *g),
              brent      (double s[], Geometry *g, Point a, Vector ap,
        double tol);

static Geometry *lookupGeom (char *name),
                *loadGeom   (char *name);

static Geometry *geomlist;

static Point setPoint (double x, double y)
{
  Point p;
  p.x = x;
  p.y = y;
  return p;
}


static Vector setVector (Point p1, Point p2)
{
  Vector v;

  v.x      = p2.x - p1.x;
  v.y      = p2.y - p1.y;
  v.length = sqrt (v.x*v.x + v.y*v.y);

  return v;
}

/* Compute the angle between the vector ap and the vector from a to
 * a point s on the curv.  Uses the small-angle approximation */

static double getAngle (double s, Geometry *g, Point a, Vector ap)
{
  Point  c;
  Vector ac;

  c  = setPoint (splint(g->npts, s, g->arclen, g->x, g->sx),
                 splint(g->npts, s, g->arclen, g->y, g->sy));
  ac = setVector(a, c);

  return 1. - ((ap.x * ac.x + ap.y * ac.y) / (ap.length * ac.length));
}

/* Search for the named Geometry */

static Geometry *lookupGeom (char *name)
{
  Geometry *g = geomlist;

  while (g) {
    if (strcmp(name, g->name) == 0)
      return g;
    g = g->next;
  }

  return (Geometry *) NULL;
}

/* Load a geometry file */

static Geometry *loadGeom (char *name){
  const int verbose = option("verbose");
  Geometry *g   = (Geometry *) calloc (1, sizeof(Geometry));
  char      buf [BUFSIZ];
  double    tmp[_MAX_NC];
  Point     p1, p2, p3, p4;
  FILE     *fp;
  register  int i;
  double  xscal = dparam("XSCALE");
  double  yscal = dparam("YSCALE");
  double  xmove = dparam("XMOVE");
  double  ymove = dparam("YMOVE");

  if (verbose > 1)
    printf ("Loading geometry file %s...", name);
  if ((fp = fopen(name, "r")) == (FILE *) NULL) {
    fprintf (stderr, "couldn't find the curved-side file %s", name);
    exit (-1);
  }

  while (fgets (buf, BUFSIZ, fp))    /* Read past the comments */
    if (*buf != '#') break;

  /* Allocate space for the coordinates */

  g -> x = (double*) calloc (_MAX_NC, sizeof(double));
  g -> y = (double*) calloc (_MAX_NC, sizeof(double));

  strcpy (g->name = (char *) malloc (strlen(name)+1), name);

  /* Read the coordinates.  The first line is already in *
   * the input buffer from the comment loop above.       */

  i = 0;
  while (i <= _MAX_NC && sscanf (buf,"%lf%lf", g->x + i, g->y + i) == 2) {
    i++;
    if (!fgets(buf, BUFSIZ, fp)) break;
  }
  g->npts = i;

  if(xmove)  dsadd(g->npts,xmove,g->x,1,g->x,1);
  if(ymove)  dsadd(g->npts,ymove,g->y,1,g->y,1);
  if(xscal)  dscal(g->npts,xscal,g->x,1);
  if(yscal)  dscal(g->npts,yscal,g->y,1);

  if (i < 2 ) error_msg (geometry file does not have enough points);

  if (i > _MAX_NC) error_msg (geometry file has too many points);

  if (verbose > 1) printf ("%d points", g->npts);

  /* Allocate memory for the other quantities */

  g->sx     = (double*) calloc (g->npts, sizeof(double));
  g->sy     = (double*) calloc (g->npts, sizeof(double));
  g->arclen = (double*) calloc (g->npts, sizeof(double));

  /* Compute spline information for the (x,y)-coordinates.  The vector "tmp"
     is a dummy independent variable for the function x(eta), y(eta).  */

  tmp[0] = 0.;
  tmp[1] = 1.;
  dramp  (g->npts, tmp, tmp + 1, tmp, 1);
  spline (g->npts, 1.e30, 1.e30, tmp, g->x, g->sx);
  spline (g->npts, 1.e30, 1.e30, tmp, g->y, g->sy);

  /* Compute the arclength of the curve using 4 points per segment */

  for (i = 0; i < (*g).npts-1; i++) {
    p1 = setPoint (g->x[i], g->y[i] );
    p2 = setPoint (splint (g->npts, i+.25, tmp, g->x, g->sx),
       splint (g->npts, i+.25, tmp, g->y, g->sy));
    p3 = setPoint (splint (g->npts, i+.75, tmp, g->x, g->sx),
       splint (g->npts, i+.75, tmp, g->y, g->sy));
    p4 = setPoint (g->x[i+1], g->y[i+1]);

    g->arclen [i+1] = g->arclen[i] + distance (p1, p2) + distance (p2, p3) +
                                     distance (p3, p4);
  }

  /* Now that we have the arclength, compute x(s), y(s) */

  spline (g->npts, 1.e30, 1.e30, g->arclen, g->x, g->sx);
  spline (g->npts, 1.e30, 1.e30, g->arclen, g->y, g->sy);

  if (verbose > 1)
    printf (", arclength  = %f\n", g->arclen[i]);


  /* add to the list of geometries */

  g ->next = geomlist;
  geomlist = g;

  fclose (fp);
  return g;
}

/*
 * Find the point at which a line passing from the anchor point "a"
 * through the search point "p" intersects the curve defined by "g".
 * Always searches from the last point found to the end of the curve.
 */

static double searchGeom (Point a, Point p, Geometry *g)
{
  Vector   ap;
  double   tol = dparam("TOLCURV"), s[3], f[3];
  register int ip;

  /* start the search at the closest point */

  ap   = setVector (a, p);
  s[0] = g -> arclen[ip = closest (p, g)];
  s[1] = g -> arclen[ip + 1];

  bracket (s, f, g, a, ap);
  if (fabs(f[1]) > tol)
    brent (s, g, a, ap, tol);

  return s[1];
}

int id_min(int n, double *d, int skip);
/* ---------------  Bracketing and Searching routines  --------------- */

static int closest (Point p, Geometry *g)
{
  const
  double  *x = g->x    + g->pos,
          *y = g->y    + g->pos;
  const    int n = g->npts - g->pos;
  double   len[_MAX_NC];
  register int i;

  for (i = 0; i < n; i++)
    len[i] = sqrt (pow(p.x - x[i],2.) + pow(p.y - y[i],2.));

  i = id_min (n, len, 1) + g->pos;
  i = min(i, g->npts-2);

  /* If we found the same position and it's not the very first *
   * one, start the search over at the beginning again.  The   *
   * test for i > 0 makes sure we only do the recursion once.  */

  if (i && i == g->pos) { g->pos = 0; i = closest (p, g); }

  return g->pos = i;
}

#define GOLD      1.618034
#define CGOLD     0.3819660
#define GLIMIT    100.
#define TINY      1.e-20
#define ZEPS      1.0e-10
#define ITMAX     100

#define SIGN(a,b)     ((b) > 0. ? fabs(a) : -fabs(a))
#define SHFT(a,b,c,d)  (a)=(b);(b)=(c);(c)=(d);
#define SHFT2(a,b,c)   (a)=(b);(b)=(c);

#define fa f[0]
#define fb f[1]
#define fc f[2]
#define xa s[0]
#define xb s[1]
#define xc s[2]

static void bracket (double s[], double f[], Geometry *g, Point a, Vector ap)
{
  double ulim, u, r, q, fu;

  fa = getAngle (xa, g, a, ap);
  fb = getAngle (xb, g, a, ap);

  if (fb > fa) { SHFT (u, xa, xb, u); SHFT (fu, fb, fa, fu); }

  xc = xb + GOLD*(xb - xa);
  fc = getAngle (xc, g, a, ap);

  while (fb > fc) {
    r = (xb - xa) * (fb - fc);
    q = (xb - xc) * (fb - fa);
    u =  xb - ((xb - xc) * q - (xb - xa) * r) /
              (2.*SIGN(max(fabs(q-r),TINY),q-r));
    ulim = xb * GLIMIT * (xc - xb);

    if ((xb - u)*(u - xc) > 0.) {      /* Parabolic u is bewteen b and c */
      fu = getAngle (u, g, a, ap);
      if (fu < fc) {                    /* Got a minimum between b and c */
  SHFT2 (xa,xb, u);
  SHFT2 (fa,fb,fu);
  return;
      } else if (fu > fb) {             /* Got a minimum between a and u */
  xc = u;
  fc = fu;
  return;
      }
      u  = xc + GOLD*(xc - xb);    /* Parabolic fit was no good. Use the */
      fu = getAngle (u, g, a, ap);             /* default magnification. */

    } else if ((xc-u)*(u-ulim) > 0.) {   /* Parabolic fit is bewteen c   */
      fu = getAngle (u, g, a, ap);                         /* and ulim   */
      if (fu < fc) {
  SHFT  (xb, xc, u, xc + GOLD*(xc - xb));
  SHFT  (fb, fc, fu, getAngle(u, g, a, ap));
      }
    } else if ((u-ulim)*(ulim-xc) >= 0.) {  /* Limit parabolic u to the  */
      u   = ulim;                           /* maximum allowed value     */
      fu  = getAngle (u, g, a, ap);
    } else {                                       /* Reject parabolic u */
      u   = xc + GOLD * (xc - xb);
      fu  = getAngle (u, g, a, ap);
    }
    SHFT  (xa, xb, xc, u);      /* Eliminate the oldest point & continue */
    SHFT  (fa, fb, fc, fu);
  }
  return;
}

/* Brent's algorithm for parabolic minimization */

static double brent (double s[], Geometry *g, Point ap, Vector app, double tol)
{
  int    iter;
  double a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm;
  double e=0.0;

  a  = min (xa, xc);               /* a and b must be in decending order */
  b  = max (xa, xc);
  d  = 1.;
  x  = w  = v  = xb;
  fw = fv = fx = getAngle (x, g, ap, app);

  for (iter = 1; iter <= ITMAX; iter++) {    /* ....... Main Loop ...... */
    xm   = 0.5*(a+b);
    tol2 = 2.0*(tol1 = tol*fabs(x)+ZEPS);
    if (fabs(x-xm) <= (tol2-0.5*(b-a))) {             /* Completion test */
      xb = x;
      return fx;
    }
    if (fabs(e) > tol1) {             /* Construct a trial parabolic fit */
      r = (x-w) * (fx-fv);
      q = (x-v) * (fx-fw);
      p = (x-v) * q-(x-w) * r;
      q = (q-r) * 2.;
      if (q > 0.) p = -p;
      q = fabs(q);
      etemp=e;
      e = d;

      /* The following conditions determine the acceptability of the    */
      /* parabolic fit.  Following we take either the golden section    */
      /* step or the parabolic step.                                    */

      if (fabs(p) >= fabs(.5*q*etemp) || p <= q*(a-x) || p >= q*(b-x))
  d = CGOLD * (e = (x >= xm ? a-x : b-x));
      else {
  d = p / q;
  u = x + d;
  if (u-a < tol2 || b-u < tol2)
    d = SIGN(tol1,xm-x);
      }
    } else
      d = CGOLD * (e = (x >= xm ? a-x : b-x));

    u  = (fabs(d) >= tol1 ? x+d : x+SIGN(tol1,d));
    fu = getAngle(u,g,ap,app);

    /* That was the one function evaluation per step.  Housekeeping... */

    if (fu <= fx) {
      if (u >= x) a = x; else b = x;
      SHFT(v ,w ,x ,u );
      SHFT(fv,fw,fx,fu)
    } else {
      if (u < x) a=u; else b=u;
      if (fu <= fw || w == x) {
  v  = w;
  w  = u;
  fv = fw;
  fw = fu;
      } else if (fu <= fv || v == x || v == w) {
  v  = u;
  fv = fu;
      }
    }
  }                        /* .......... End of the Main Loop .......... */

  error_msg(too many iterations in brent());
  xb = x;
  return fx;
}

#undef ITMAX
#undef CGOLD
#undef ZEPS
#undef SIGN
#undef fa
#undef fb
#undef fc
#undef xa
#undef xb
#undef xc
