TagHandle TargetWriter::get_tag_handle( const std::string& base_name,
                                        unsigned num_dbl, Mesh* mesh, MsqError& err )
{
  std::ostringstream sstr;
  sstr << base_name << num_dbl;
  
  TagHandle handle = mesh->tag_get( sstr.str().c_str(), err );
  if (!MSQ_CHKERR(err))
  {
    std::string temp_name;
    Mesh::TagType temp_type;
    unsigned temp_length;
    mesh->tag_properties( handle, temp_name, temp_type, temp_length, err );
    MSQ_ERRZERO(err);
    
    if (temp_type != Mesh::DOUBLE || temp_length != num_dbl)
    {
      MSQ_SETERR(err)( MsqError::TAG_ALREADY_EXISTS,
                      "Mismatched type or length for existing tag \"%s\"",
                       sstr.str().c_str() );
    }
  }
  else if (err.error_code() == MsqError::TAG_NOT_FOUND)
  {
    err.clear();
    handle = mesh->tag_create( sstr.str().c_str(), Mesh::DOUBLE, num_dbl, 0, err );
    MSQ_ERRZERO(err);
  }
  return handle;
}

static TagHandle get_tag( Mesh* mesh,
                          unsigned num_doubles,
                          const std::string& base_name,
                          MsqError& err )
{
  std::ostringstream str;
  str << base_name << num_doubles;
  
  TagHandle handle = mesh->tag_get( str.str().c_str(), err ); MSQ_ERRZERO(err);
  
    // double check tag type
  std::string temp_name;
  Mesh::TagType temp_type;
  unsigned temp_length;
  mesh->tag_properties( handle, temp_name, temp_type, temp_length, err );
  MSQ_ERRZERO(err);

  if (temp_type != Mesh::DOUBLE || temp_length != num_doubles)
  {
    MSQ_SETERR(err)( MsqError::TAG_ALREADY_EXISTS,
                    "Mismatched type or length for existing tag \"%s\"",
                     str.str().c_str() );
  }
  
  return handle;
}

bool TQualityMetric::evaluate_internal( PatchData& pd,
                                        size_t handle,
                                        double& value,
                                        size_t* indices,
                                        size_t& num_indices,
                                        MsqError& err )
{
  const Sample s = ElemSampleQM::sample( handle );
  const size_t e = ElemSampleQM::  elem( handle );
  MsqMeshEntity& elem = pd.element_by_index( e );
  EntityTopology type = elem.get_element_type();
  unsigned edim = TopologyInfo::dimension( type );
  const NodeSet bits = pd.non_slave_node_set( e );
  
  bool rval;
  if (edim == 3) { // 3x3 or 3x2 targets ?
    const MappingFunction3D* mf = pd.get_mapping_function_3D( type );
    if (!mf) {
      MSQ_SETERR(err)( "No mapping function for element type", MsqError::UNSUPPORTED_ELEMENT );
      return false;
    }

    MsqMatrix<3,3> A, W;
    mf->jacobian( pd, e, bits, s, indices, mDerivs3D, num_indices, A, err );
    MSQ_ERRZERO(err);
    targetCalc->get_3D_target( pd, e, s, W, err ); MSQ_ERRZERO(err);
    const MsqMatrix<3,3> Winv = inverse(W);
    const MsqMatrix<3,3> T = A*Winv;
    rval = targetMetric->evaluate( T, value, err ); MSQ_ERRZERO(err);
#ifdef PRINT_INFO
    print_info<3>( e, s, A, W, A * inverse(W) );
#endif
  }
  else if (edim == 2) {
    MsqMatrix<2,2> W, A;
    MsqMatrix<3,2> S_a_transpose_Theta;
    rval = evaluate_surface_common( pd, s, e, bits, indices, num_indices,
                                 mDerivs2D, W, A, S_a_transpose_Theta, err ); 
    if (MSQ_CHKERR(err) || !rval)
      return false;
    const MsqMatrix<2,2> Winv = inverse(W);
    const MsqMatrix<2,2> T = A * Winv;
    rval = targetMetric->evaluate( T, value, err );
    MSQ_ERRZERO(err);

#ifdef PRINT_INFO
    print_info<2>( e, s, J, Wp, A * inverse(W) );
#endif
  }
  else {
    assert(false);
    return false;
  }
  
  return rval;
}

bool AddQualityMetric::evaluate( PatchData& pd,
                                 size_t handle,
                                 double& value,
                                 MsqError& err )
{
  double val1, val2;
  bool rval1, rval2;
  rval1 = metric1.evaluate( pd, handle, val1, err ); MSQ_ERRZERO(err);
  rval2 = metric2.evaluate( pd, handle, val2, err ); MSQ_ERRZERO(err);
  value = val1 + val2;
  return rval1 && rval2;
}

bool TopologyInfo::compare_sides( const size_t* verts1,
                               EntityTopology type1,
                               unsigned side1,
                               const size_t* verts2,
                               EntityTopology type2,
                               unsigned side2,
                               unsigned side_dim,
                               MsqError& err )
{
  const unsigned *conn1, *conn2;
  unsigned len1, len2;
  
  conn1 = side_vertices( type1, side_dim, side1, len1, err );
  MSQ_ERRZERO(err);
  conn2 = side_vertices( type2, side_dim, side2, len2, err );
  MSQ_ERRZERO(err);
  
    // obviously not the same if different number of vertices
    // (triangular face cannot match quadrilateral face)
  if (len1 != len2)
    return false;
  
    // Find location (i) in vertices of side of second element
    // that matches the first vertex in the side of the first 
    // element.
  unsigned i, j;
  for (i = 0; i < len2; ++i)
    if (verts1[conn1[0]] == verts2[conn2[i]])
      break;
    // If not found, then no match
  if (i == len2)
    return false;
  
    // Try comparing side connectivity in forward order
  for (j = 1; j < len1; ++j)
    if (verts1[conn1[j]] != verts2[conn2[(i+j)%len2]])
      break;
    // If they match, we're done
  if (j == len1)
    return true;
  
    // Try comparing in reverse order
  for (j = 1; j < len1; ++j)
    if (verts1[conn1[j]] != verts2[conn2[(i+len2-j)%len2]])
      return false;
    // If here, matched in reverse order
  return true;
}

bool ObjectiveFunction::compute_subpatch_numerical_gradient(
                                 EvalType type,
                                 EvalType subtype,
                                 PatchData& pd,
                                 double& flocal,
                                 Vector3D& grad,
                                 MsqError& err )
{
  assert( pd.num_free_vertices() == 1 );
  
  double flocald=0;
  double eps=0;
  
  bool b = evaluate( type, pd, flocal, OF_FREE_EVALS_ONLY, err );
  if(MSQ_CHKERR(err) || !b) {
    return false;
  }

    //loop over the three coords x,y,z
  for(int j=0; j<3; ++j) {
    eps = get_eps( pd, subtype, flocald, j, 0, err ); MSQ_ERRZERO(err);
    if (eps==0) {
      MSQ_SETERR(err)("Dividing by zero in Objective Functions numerical grad",
                      MsqError::INVALID_STATE);
      return false;
    }
    grad[j]=(flocald-flocal)/eps;
  }
  return true;
}

/*!Returns an appropiate value (eps) to use as a delta step for
  MsqVertex vertex in dimension k (i.e. k=0 -> x, k=1 -> y, k=2 -> z).
  The objective function value at the perturbed vertex position is given
  in local_val.
*/
double ObjectiveFunction::get_eps( PatchData &pd, 
                                   EvalType type,
                                   double &local_val,
                                   int dim,
                                   size_t vertex_index, 
                                   MsqError& err)
{
  double eps = 1.e-07;
  const double rho = 0.5;
  const int imax = 20;
  bool feasible = false;
  double tmp_var = 0.0;
  Vector3D delta(0,0,0);
  for (int i = 0; i < imax; ++i)
  {
    //perturb kth coord val and check feas if needed
    tmp_var = pd.vertex_by_index(vertex_index)[dim];
    delta[dim] = eps;
    pd.move_vertex( delta, vertex_index, err );
    feasible = evaluate( type, pd, local_val, OF_FREE_EVALS_ONLY, err ); MSQ_ERRZERO(err);
    //revert kth coord val
    delta = pd.vertex_by_index(vertex_index);
    delta[dim] = tmp_var;
    pd.set_vertex_coordinates( delta, vertex_index, err );
    //if step was too big, shorten it and go again
    if (feasible)
      return eps;
    else
      eps *= rho;
  }//end while looking for feasible eps
  return 0.0;
}//end function get_eps

bool ObjectiveFunction::evaluate_with_Hessian_diagonal( EvalType type, 
                                        PatchData& pd,
                                        double& value_out,
                                        std::vector<Vector3D>& grad_out,
                                        std::vector<SymMatrix3D>& hess_diag_out,
                                        MsqError& err )
{
  MsqHessian hess;
  hess.initialize( pd, err ); MSQ_ERRZERO(err);
  bool val = evaluate_with_Hessian( type, pd, value_out, grad_out, hess, err );
  MSQ_ERRZERO(err);
  hess_diag_out.resize( hess.size() );
  for (size_t i = 0; i < hess.size(); ++i)
    hess_diag_out[i] = hess.get_block(i,i)->upper();
  return val;
}

double 
NonGradient::evaluate( double *point,  PatchData &pd, MsqError &err )
{
  if( pd.num_free_vertices() > 1 )
  {
    MSQ_SETERR(err)("Only one free vertex per patch implemented", MsqError::NOT_IMPLEMENTED);
  }
  const size_t vertexIndex = 0; 
  Vector3D originalVec = pd.vertex_by_index(vertexIndex);
  Vector3D pointVec;
  for( int index = 0; index<3; index++)
  {
    pointVec[index] = point[index];
  }
  pd.set_vertex_coordinates( pointVec, vertexIndex, err ); 
  pd.snap_vertex_to_domain( vertexIndex, err );  

  OFEvaluator& obj_func = get_objective_function_evaluator();
  TerminationCriterion* term_crit=get_inner_termination_criterion();

  double value;
  bool feasible = obj_func.evaluate( pd, value, err ); // MSQ_ERRZERO(err);
  term_crit->accumulate_inner( pd, value, 0, err );  //MSQ_CHKERR(err);
  if (err.error_code() == err.BARRIER_VIOLATED)
    err.clear();   // barrier violation not an error here
  MSQ_ERRZERO(err);
  pd.set_vertex_coordinates( originalVec, vertexIndex, err ); 
  if( !feasible && mUseExactPenaltyFunction )  
  { // "value" undefined btw
    double ensureFiniteRtol= .25;
    value = DBL_MAX * ensureFiniteRtol;
    //std::cout << " Infeasible patch: " << value << std::endl; printPatch( pd, err );
  }
  return value;
}

unsigned TopologyInfo::find_edge( EntityTopology topo,
                                  const unsigned* side_vertices,
                                  bool& reversed_out,
                                  MsqError& err )
{
  if (dimension(topo) <= 1) {
    MSQ_SETERR(err)(MsqError::INVALID_ARG,"Invalid element dimension");
    return (unsigned)-1;
  }

  for (unsigned i = 0; i < edges(topo); ++i) {
    const unsigned* edge = edge_vertices( topo, i, err );
    MSQ_ERRZERO(err);

    if (edge[0] == side_vertices[0] &&
        edge[1] == side_vertices[1]) {
      reversed_out = false;
      return i;
    }

    if (edge[0] == side_vertices[1] &&
        edge[1] == side_vertices[0]) {
      reversed_out = true;
      return i;
    }
  }
  
  MSQ_SETERR(err)(MsqError::INVALID_ARG,"No such edge");
  return (unsigned)-1;
}

bool PatchPowerMeanP::initialize_block_coordinate_descent( Mesh* mesh, 
                                                      MeshDomain* domain, 
                                                      const Settings* settings,
                                                      PatchSet* patch_set,
                                                      MsqError& err )
{
  clear();
  PatchIterator patches( patch_set );
  
  PatchData pd;
  pd.set_mesh( mesh );
  pd.set_domain( domain );
  if (settings)
    pd.attach_settings(settings);
  
  bool result = true;
  while (patches.get_next_patch( pd, err ) && !MSQ_CHKERR(err))
  {
    double value;
    bool b = evaluate( ObjectiveFunction::ACCUMULATE, pd, value, false, err ); 
    MSQ_ERRZERO(err);
    result = result && b;
  }
  return result;
}

bool CachingTargetCalculator::get_surface_target( PatchData& pd, 
                                             size_t element,
                                             Sample sample,
                                             MsqMatrix<3,2>& W_out,
                                             MsqError& err )
{
  CachedTargetData& data = get_data( pd );
  if (data.targetsSurface.empty()) {
    if (!have_surface_orient()) {
      MSQ_SETERR(err)("Incorrect surface mesh target type", MsqError::INTERNAL_ERROR );
      return false;
    }
  
    populate_data( pd, &data, cachedCalculator, err );
    MSQ_ERRZERO(err);
    if (data.targetsSurface.empty()) {
      MSQ_SETERR(err)("Attempt to get 2D target for 3D element type", MsqError::INVALID_STATE);
      return false;
    }
  }
  
    // calculate index of sample in array 
  NodeSet all_samples = pd.get_samples( element );
  unsigned offset = all_samples.num_before( sample );

  W_out = data.targetsSurface[ data.elementOffsets[element] + offset ];
  return true;
}

bool TMPQualityMetric::evaluate_with_indices( PatchData& pd,
        size_t handle,
        double& value,
        std::vector<size_t>& indices,
        MsqError& err )
{
    indices.resize( MAX_ELEM_NODES );
    size_t num_idx = 0;
    bool result = evaluate_internal( pd, handle, value, arrptr(indices), num_idx, err );
    if (MSQ_CHKERR(err) || !result)
        return false;

    indices.resize( num_idx );

    // apply target weight to value
    if (weightCalc) {
        const Sample s = ElemSampleQM::sample( handle );
        const size_t e = ElemSampleQM::  elem( handle );
        double ck = weightCalc->get_weight( pd, e, s, err );
        MSQ_ERRZERO(err);
        value *= ck;
    }

    return true;
}

bool ElementAvgQM::evaluate( PatchData& pd, 
                              size_t handle, 
                              double& value, 
                              MsqError& err )
{
  ElemSampleQM* qm = get_quality_metric();
  mHandles.clear();
  qm->get_element_evaluations( pd, handle, mHandles, err ); MSQ_ERRFALSE(err);

  bool valid = true;
  double tmpval;
  double accumulate = 0.0;
  int num_values = 0;
  bool tmpvalid;

  value = -std::numeric_limits<double>::infinity();
  for (std::vector<size_t>::iterator h = mHandles.begin(); h != mHandles.end(); ++h) 
  {
    tmpvalid = qm->evaluate( pd, *h, tmpval, err ); MSQ_ERRZERO(err);
    if (!tmpvalid) 
    {
      valid = false;
      break;
    }
    else
    { 
      accumulate += tmpval;    
      num_values++;
    }
  }
  if (valid)
    value = accumulate/num_values;
    
  return valid;
}

int TopologyInfo::higher_order_from_side( EntityTopology topo,
                                          unsigned num_nodes,
                                          unsigned side_dimension,
                                          unsigned side_number,
                                          MsqError& err )
{
  bool mids[4] = { true };
  higher_order( topo, num_nodes, mids[1], mids[2], mids[3], err );
  MSQ_ERRZERO(err);
  
  if (side_dimension > dimension(topo) || 
      side_number > adjacent(topo, side_dimension)) {
    MSQ_SETERR(err)(MsqError::INVALID_ARG,"Invalid side number: %u\n", side_number );
    return 0;
  }
  
  if (!mids[side_dimension])
    return -1;
  
  int result = side_number;
  switch (side_dimension) {
    case 3: if (mids[2]) result += faces(topo);
    case 2: if (mids[1]) result += edges(topo);
    case 1: result += corners(topo);
    case 0: break;
    default: 
      MSQ_SETERR(err)(MsqError::INVALID_ARG,"Invalid dimension: %u\n", side_dimension );
      return 0;
  }
  return result;
}

bool TMixed::evaluate( const MsqMatrix<3,3>& T, 
                       double& result, 
                       MsqError& err )
{
  bool rval = mu3D->evaluate( T, result, err );
  MSQ_ERRZERO(err);
  return rval;
}

/*! 
  Numerically Calculates the gradient of the ObjectiveFunction for the
  free vertices in the patch.  Returns 'false' if the patch is outside
  of a required feasible region, returns 'ture' otherwise.
  The behavior of the function depends on the value of the boolean
  useLocalGradient.  If useLocalGradient is set to
  'true', compute_numerical_gradient creates a sub-patch around a free
  vertex, and then perturbs that vertex in one of the coordinate directions.
  Only the ObjectiveFunction value on the local sub-patch is used in the
  computation of the gradient.  Therefore, useLocalGradient should only
  be set to 'true' for ObjectiveFunctions which can use this method.  Unless
  the concrete ObjectiveFunction sets useLocalGradient to 'true' in its
  constructor, the value will be 'false'.  In this case, the objective
  function value for the entire patch is used in the calculation of the
  gradient.  This is computationally expensive, but it is numerically
  correct for all (C_1) functions.
  \param pd  PatchData on which the gradient is taken.
  \param grad  Array of Vector3D of length the number of vertices used to store gradient.
  \param OF_val will be set to the objective function value.
 */
bool ObjectiveFunction::evaluate_with_gradient( EvalType eval_type,
                                                PatchData &pd,
                                                double& OF_val,
                                                std::vector<Vector3D>& grad,
                                                MsqError &err )
{
  bool b;
  grad.resize( pd.num_free_vertices() );
  
    // Fast path for single-free-vertex patch
  if (pd.num_free_vertices() == 1) {
    const EvalType sub_type = (eval_type == CALCULATE) ? CALCULATE : TEMPORARY;
    b = compute_subpatch_numerical_gradient( eval_type, sub_type, pd, OF_val, grad[0], err );
    return !MSQ_CHKERR(err) && b;
  }
  
  ObjectiveFunction* of = this;
  std::auto_ptr<ObjectiveFunction> deleter;
  if (eval_type == CALCULATE) {
    of->clear();
    b = of->evaluate( ACCUMULATE, pd, OF_val, OF_FREE_EVALS_ONLY, err );
    if (err) { // OF doesn't support BCD type evals, try slow method
      err.clear();
      of->clear();
      b = compute_patch_numerical_gradient( CALCULATE, CALCULATE, pd, OF_val, grad, err );
      return !MSQ_CHKERR(err) && b;
    }
    else if (!b)
      return b;
  } 
  else {
    b = this->evaluate( eval_type, pd, OF_val, OF_FREE_EVALS_ONLY, err );
    if (MSQ_CHKERR(err) || !b)
      return false;
    of = this->clone();
    deleter = std::auto_ptr<ObjectiveFunction>(of);
  }

    // Determine number of layers of adjacent elements based on metric type.
  unsigned layers = min_patch_layers();
  
    // Create a subpatch for each free vertex and use it to evaluate the
    // gradient for that vertex.
  double flocal;
  PatchData subpatch;
  for (size_t i = 0; i < pd.num_free_vertices(); ++i)
  {
    pd.get_subpatch( i, layers, subpatch, err ); MSQ_ERRZERO(err);
    b = of->compute_subpatch_numerical_gradient( SAVE, TEMPORARY, subpatch, flocal, grad[i], err );
    if (MSQ_CHKERR(err) || !b) {
      of->clear();
      return false;
    }
  }
  
  of->clear();
  return true;
}

bool TScale::evaluate( const MsqMatrix<3,3>& T, 
                       double& result, 
                       MsqError& err )
{
  bool rval = mMetric->evaluate( T, result, err );
  MSQ_ERRZERO(err);
  result *= mAlpha;
  return rval;
}

size_t SmartLaplacianSmoother::num_inverted( PatchData& pd, MsqError& err )
{
  size_t result = 0;
  int inverted, junk;
  for (size_t i = 0; i < pd.num_elements(); ++i) {
    pd.element_by_index(i).check_element_orientation( pd, inverted, junk, err ); 
    MSQ_ERRZERO(err);
    if (inverted)
      ++result;
  }
  return result;
}

bool CompositeOFAdd::initialize_block_coordinate_descent( 
                                                       MeshDomainAssoc* mesh_and_domain,
                                                       const Settings* settings,
                                                       PatchSet* user_set,
                                                       MsqError& err )
{
  bool rval1, rval2;
  rval1 = objFunc1->initialize_block_coordinate_descent( mesh_and_domain, settings, user_set, err );
  MSQ_ERRZERO(err);
  rval2 = objFunc2->initialize_block_coordinate_descent( mesh_and_domain, settings, user_set, err );
  return !MSQ_CHKERR(err) && rval1 && rval2;
}