/*! 
  \brief Calls compute_vertex_numerical_gradient if gradType equals
  NUMERCIAL_GRADIENT.  Calls compute_vertex_analytical_gradient if 
  gradType equals ANALYTICAL_GRADIENT;
*/
   inline bool QualityMetric::compute_vertex_gradient(PatchData &pd,
                                                      MsqVertex &vertex,
                                                      MsqVertex* vertices[],
                                                      Vector3D grad_vec[],
                                                      int num_vtx,
                                                      double &metric_value,
                                                      MsqError &err)
   {
     bool ret=false;;
     switch(gradType)
     {
       case NUMERICAL_GRADIENT:
          ret = compute_vertex_numerical_gradient(pd, vertex, vertices,
                                                  grad_vec, num_vtx,
                                                  metric_value, err);
          MSQ_CHKERR(err);
          break;
       case ANALYTICAL_GRADIENT:
          ret = compute_vertex_analytical_gradient(pd, vertex, vertices,
                                                   grad_vec,num_vtx,
                                                   metric_value, err);
          MSQ_CHKERR(err);
          break;
     }
     return ret;
   }

bool CompositeOFAdd::evaluate_with_gradient( EvalType type, 
                                             PatchData& pd,
                                             double& value_out,
                                             std::vector<Vector3D>& grad_out,
                                             MsqError& err )
{
  double value_2;
  bool ok;
  
  ok = objFunc1->evaluate_with_gradient( type, pd, value_out, grad_out, err );
  if (MSQ_CHKERR(err) || !ok) return false;
  ok = objFunc2->evaluate_with_gradient( type, pd, value_2, mGradient, err );
  if (MSQ_CHKERR(err) || !ok) return false;
  
  assert( grad_out.size() == pd.num_free_vertices() );
  assert( mGradient.size() == pd.num_free_vertices() );
  
  std::vector<Vector3D>::iterator i = grad_out.begin(), j = mGradient.begin();
  while (i != grad_out.end()) {
    *i += *j;
    ++i;
    ++j;
  }
  value_out += value_2;
  return true;
}

bool LVQDTargetCalculator::evaluate_guide_2D( PatchData& pd, 
                                              size_t element,
                                              Sample sample,
                                              int idx,
                                              double& lambda, 
                                              MsqMatrix<3,2>& V,
                                              MsqMatrix<2,2>& Q,
                                              MsqMatrix<2,2>& delta,
                                              MsqError& err )
{
  bool valid;
  if (uniqueGuides[idx]->have_surface_orient()) {
    MsqMatrix<3,2> W;
    valid = uniqueGuides[idx]->get_surface_target( pd, element, sample, W, err );
    if (MSQ_CHKERR(err) || !valid)
      return false;
    valid = factor_surface( W, lambda, V, Q, delta, err );
    if (MSQ_CHKERR(err) || !valid)
      return false;
  }
  else {
    MsqMatrix<2,2> W;
    valid = uniqueGuides[idx]->get_2D_target( pd, element, sample, W, err );
    if (MSQ_CHKERR(err) || !valid)
      return false;
    MsqMatrix<2,2> junk;
    valid = factor_2D( W, lambda, junk, Q, delta, err );
    if (MSQ_CHKERR(err) || !valid)
      return false;
    V = MsqMatrix<3,2>(1.0);
  }
  return true;
}

bool CompositeOFMultiply::evaluate_with_Hessian_diagonal( EvalType type, 
                                            PatchData& pd,
                                            double& value_out,
                                            std::vector<Vector3D>& grad_out,
                                            std::vector<SymMatrix3D>& diag_out,
                                            MsqError& err )
{
  double value_2;
  bool valid;

  valid = objFunc1->evaluate_with_Hessian_diagonal( type, pd, value_out, grad_out, diag_out, err );
  if (MSQ_CHKERR(err) || !valid) return false;
  valid = objFunc2->evaluate_with_Hessian_diagonal( type, pd, value_2, mGradient, mDiagonal, err );
  if (MSQ_CHKERR(err) || !valid) return false;

  for (size_t i = 0; i < pd.num_free_vertices(); ++i) {
    diag_out[i] *= value_2;
    mDiagonal[i] *= value_out;
    diag_out[i] += mDiagonal[i];
    diag_out[i] += outer_plus_transpose( grad_out[i], mGradient[i] );

    grad_out[i] *= value_2;
    mGradient[i] *= value_out;
    grad_out[i] += mGradient[i];
  }
  
  value_out *= value_2;
  return true;
}

bool CompositeOFAdd::evaluate_with_Hessian( EvalType type, 
                                            PatchData& pd,
                                            double& value_out,
                                            std::vector<Vector3D>& grad_out,
                                            MsqHessian& Hessian_out,
                                            MsqError& err )
{
  double value_2;
  bool ok;
  
  mHessian.initialize( Hessian_out );
  
  ok = objFunc1->evaluate_with_Hessian( type, pd, value_out, grad_out, Hessian_out, err );
  if (MSQ_CHKERR(err) || !ok) return false;
  ok = objFunc2->evaluate_with_Hessian( type, pd, value_2, mGradient, mHessian, err );
  if (MSQ_CHKERR(err) || !ok) return false;
  
  value_out += value_2;
  
  assert( grad_out.size() == pd.num_free_vertices() );
  assert( mGradient.size() == pd.num_free_vertices() );
  
  for (size_t i = 0; i < pd.num_free_vertices(); ++i) 
    grad_out[i] += mGradient[i];
  Hessian_out.add( mHessian );
  return true;
}

void TopologyInfo::find_side( EntityTopology topo, 
                              const unsigned* side_vertices,
                              unsigned num_vertices,
                              unsigned& dimension_out,
                              unsigned& number_out,
                              bool& reversed_out,
                              MsqError& err )
{
  switch (num_vertices) {
  case 1:
    dimension_out = 0;
    number_out = *side_vertices;
    reversed_out = false;
    if (*side_vertices >= corners(topo)) 
      MSQ_SETERR(err)(MsqError::INVALID_ARG,"Invalid corner number: %u\n", *side_vertices);
    break;
  case 2:
    dimension_out = 1;
    number_out = find_edge( topo, side_vertices, reversed_out, err );
    MSQ_CHKERR(err);
    break;
  case 3:
  case 4:
    dimension_out = 2;
    number_out = find_face( topo, side_vertices, num_vertices, reversed_out, err );
    MSQ_CHKERR(err);
    break;
  default:
    MSQ_SETERR(err)(MsqError::UNSUPPORTED_ELEMENT, "Invalid number of side vertices: %u\n", num_vertices );
    break;
  }
}

/*! 
  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 LVQDTargetCalculator::get_3D_target( PatchData& pd, 
                                          size_t element,
                                          Sample sample,
                                          MsqMatrix<3,3>& W_out,
                                          MsqError& err )
{
  double lambda[4];
  MsqMatrix<3,3> V[4], Q[4], delta[4], W;
  bool valid;
  
  for (int i = 0; i < numUniqueGuides; ++i) {
    valid = uniqueGuides[i]->get_3D_target( pd, element, sample, W, err );
    if (MSQ_CHKERR(err) || !valid)
      return false;
    valid = factor_3D( W, lambda[i], V[i], Q[i], delta[i], err );
    if (MSQ_CHKERR(err) || !valid)
      return false;
  }
  
  if (vIdx >= 0) {
    W_out = V[vIdx];
    if (lambdaIdx >= 0)
      W_out *= lambda[lambdaIdx];
    if (qIdx >= 0)
      W_out = W_out * Q[qIdx];
    if (deltaIdx >= 0)
      W_out = W_out * delta[deltaIdx];
  }
  else if (qIdx >= 0) {
    W_out = Q[qIdx];
    if (lambdaIdx >= 0)
      W_out *= lambda[lambdaIdx];
    if (deltaIdx >= 0)
      W_out = W_out * delta[deltaIdx];
  }
  else if (deltaIdx >= 0) {
    W_out = delta[deltaIdx];
    if (lambdaIdx >= 0)
      W_out *= lambda[lambdaIdx];
  }
  else if (lambdaIdx >= 0) {
    W_out = MsqMatrix<3,3>(lambda[lambdaIdx]);
  }
  else {
    W_out = MsqMatrix<3,3>(1.0);
  }

  return true;
}

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

bool LInfTemplate::evaluate( EvalType type, 
                             PatchData& pd,
                             double& value_out,
                             bool free,
                             MsqError& err )
{
  if (type != ObjectiveFunction::CALCULATE) {
    MSQ_SETERR(err)(
      "LInfTemplate does not support block coodinate descent algoritms",
      MsqError::INVALID_STATE );
    return false;
  }

  QualityMetric* qm = get_quality_metric();
  qm->get_evaluations( pd, qmHandles, free, err );  MSQ_ERRFALSE(err);
  const double negate = qm->get_negate_flag();
  
    // calculate OF value for just the patch
  std::vector<size_t>::const_iterator i;
  double value;
  value_out = -HUGE_VAL;
  for (i = qmHandles.begin(); i != qmHandles.end(); ++i)
  {
    bool result = qm->evaluate( pd, *i, value, err );
    if (MSQ_CHKERR(err) || !result)
      return false;

    value = negate * fabs(value);
    if (value > value_out)
      value_out = value;
  }
  
  return true;
}

/*! \fn MeshSet::get_next_patch(PatchData &pd, MsqError &err )
  \brief This function fills up a PatchData object with the mesh information
  necessary for optimization algorithms.
  The return value is true as long there exists a next patch, false otherwise.
  The list culling is performed in this function.
  
  This function is a friend of the PatchData class. Therefore the PatchData
  object passed as an argument is filled up directly.
  
  \param PatchData &pd: this is the PatchData object that will be filled up.
*/
bool MeshSet::get_next_patch(PatchData &pd,
                             PatchDataParameters &pd_params,
                             MsqError &err )
{
  MSQ_FUNCTION_TIMER( "MeshSet::get_next_patch" );

    // get rid of previous Patch information (but keep memory allocated).
  pd.clear();

    // Mark this MeshSet as the originator
  pd.meshSet = this;
  pd.domainSet = (mDomain != NULL);

  bool result = false;
  switch (pd_params.get_patch_type())
  {
    case PatchData::ELEMENTS_ON_VERTEX_PATCH:
      result = get_next_elem_on_vert_patch( pd, pd_params, err );
      break;
    case PatchData::GLOBAL_PATCH:
      result = get_next_global_patch( pd, pd_params, err );
      break;
    default:
      MSQ_SETERR(err)( "no implementation for specified patch type.",
                       MsqError::NOT_IMPLEMENTED );
      result = false;
      break;
  }

  return !MSQ_CHKERR(err) && result;
}

void TerminationCriterion::accumulate_inner( PatchData& pd, 
                                             OFEvaluator& of_eval,
                                             MsqError& err )
{
  double of_value = 0;
  
  if (terminationCriterionFlag & GRAD_FLAGS)
  {
    mGrad.resize( pd.num_free_vertices() );
    bool b = of_eval.evaluate(pd, of_value, mGrad, err);
    MSQ_ERRRTN(err);
    if (!b) {
      MSQ_SETERR(err)("Initial patch is invalid for gradient compuation.",
                      MsqError::INVALID_MESH);
      return;
    }
  }
  else if (terminationCriterionFlag & OF_FLAGS)
  {
    bool b = of_eval.evaluate(pd, of_value, err); MSQ_ERRRTN(err);
    if (!b) {
      MSQ_SETERR(err)("Invalid patch passed to TerminationCriterion.",
                      MsqError::INVALID_MESH);
      return;
    }
  }

  accumulate_inner( pd, of_value, mGrad.empty() ? 0 : arrptr(mGrad), err );  MSQ_CHKERR(err);
}

template <unsigned DIM> inline
bool TUntangleMu::hess( const MsqMatrix<DIM,DIM>& T,
                        double& result,
                        MsqMatrix<DIM,DIM>& deriv_wrt_T,
                        MsqMatrix<DIM,DIM>* second_wrt_T,
                        MsqError& err )
{
    bool valid = mBaseMetric->evaluate_with_hess( T, result, deriv_wrt_T, second_wrt_T, err );
    if (MSQ_CHKERR(err) || !valid)
        return false;

    if (mConstant < result) {
        const double s = result - mConstant;
        result = s*s*s;
        hess_scale( second_wrt_T, 3*s*s );
        pluseq_scaled_outer_product( second_wrt_T, 6*s, deriv_wrt_T );
        deriv_wrt_T *= 3*s*s;
    }
    else {
        result = 0;
        deriv_wrt_T = MsqMatrix<DIM,DIM>(0.0);
        set_scaled_I( second_wrt_T, 0.0 ); // zero everything
    }

    return true;
}

void TagVertexMesh::initialize( Mesh* mesh, std::string name, MsqError& err )
{
  MeshDecorator::set_mesh( mesh );
  tagName = name;

  tagHandle = get_mesh()->tag_get( tagName, err );
    // If tag isn't defined yet, we're done for now.
  if (err.error_code() == MsqError::TAG_NOT_FOUND) {
    err.clear();
    return;
  } 
  else if (MSQ_CHKERR(err))
    return;
  
    // If tag is already defined, make sure it is the correct type.
  std::string t_name;
  Mesh::TagType type;
  unsigned length;
  tag_properties( tagHandle, t_name, type, length, err ); 
  MSQ_ERRRTN(err);
  if (!(type == Mesh::DOUBLE && length == 3) &&
      !(type == Mesh::BYTE && length == 3*sizeof(double)))
    MSQ_SETERR(err)(MsqError::TAG_ALREADY_EXISTS,
                    "Tag \"%s\" has invalid type or size.",
                    tagName.c_str());
 
    // If tag is already defined and init was true, reset tag
    // values.
  haveTagHandle = 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;
}

void QuasiNewton::initialize( PatchData& pd, MsqError& err )
{
  if (!mMemento) {
    mMemento = pd.create_vertices_memento( err );
    MSQ_CHKERR(err);
  }
}

bool FileTokenizer::get_double_internal( double& result, MsqError& err )
{
    // Get a token
  const char *token_end, *token = get_string( err );
  if (MSQ_CHKERR(err))
    return false;
  
    // Check for hex value -- on some platforms (e.g. Linux), strtod
    // will accept hex values, on others (e.g. Sun) it wil not.  Force
    // failure on hex numbers for consistancy.
  if (token[0] && token[1] && token[0] == '0' && toupper(token[1]) == 'X')
  {
    MSQ_SETERR(err)( MsqError::PARSE_ERROR,
      "Syntax error at line %d: expected number, got \"%s\"",
      line_number(), token );
    return false;
  }
  
  
    // Parse token as double
  result = strtod( token, (char**)&token_end );

    // If the one past the last char read by strtod is
    // not the NULL character terminating the string,
    // then parse failed.
  if (*token_end)
  {
    MSQ_SETERR(err)( MsqError::PARSE_ERROR,
      "Syntax error at line %d: expected number, got \"%s\"",
      line_number(), token );
    return false;
  }
  
  return true;
}

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 PMeanPTemplate::evaluate( EvalType type, 
                           PatchData& pd,
                           double& value_out,
                           bool free,
                           MsqError& err )
{
  QualityMetric* qm = get_quality_metric();
  if (type == ObjectiveFunction::ACCUMULATE)
    qm->get_single_pass( pd, qmHandles, free, err );
  else
    qm->get_evaluations( pd, qmHandles, free, err );  
  MSQ_ERRFALSE(err);
  
    // calculate OF value for just the patch
  std::vector<size_t>::const_iterator i;
  double value, working_sum = 0.0;
  for (i = qmHandles.begin(); i != qmHandles.end(); ++i)
  {
    bool result = qm->evaluate( pd, *i, value, err );
    if (MSQ_CHKERR(err) || !result)
      return false;
    
    working_sum += mPower.raise( value );
  }
  
    // get overall OF value, update member data, etc.
  size_t global_count;
  value_out = qm->get_negate_flag() 
            * get_value( working_sum, qmHandles.size(), type, global_count );
  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;
}