#include "drake/solvers/scs_solver.h"

#include <Eigen/Sparse>
#include <fmt/format.h>

// clang-format off
// scs.h should be included before linsys/amatrix.h, since amatrix.h uses types
// scs_float, scs_int, etc, defined in scs.h
#include <scs.h>
#include <cones.h>
#include <linalg.h>
#include <util.h>
#include "linsys/amatrix.h"
// clang-format on

#include "drake/common/text_logging.h"
#include "drake/math/eigen_sparse_triplet.h"
#include "drake/math/quadratic_form.h"
#include "drake/solvers/mathematical_program.h"

namespace drake {
namespace solvers {
namespace {
void ParseLinearCost(const MathematicalProgram& prog, std::vector<double>* c,
                     double* constant) {
  for (const auto& linear_cost : prog.linear_costs()) {
    // Each linear cost is in the form of aᵀx + b
    const auto& a = linear_cost.evaluator()->a();
    const VectorXDecisionVariable& x = linear_cost.variables();
    for (int i = 0; i < a.rows(); ++i) {
      (*c)[prog.FindDecisionVariableIndex(x(i))] += a(i);
    }
    (*constant) += linear_cost.evaluator()->b();
  }
}

// Add a rotated Lorentz cone constraint that A_cone * x + b_cone is in the
// rotated Lorentz cone.
// @param A_cone_triplets The triplets of non-zero entries in A_cone.
// @param b_cone A_cone * x + b_cone is in the rotated Lorentz cone.
// @param x_indices The index of the variable in SCS.
// @param A_triplets[in/out] The non-zero entry triplet in A before and after
// adding the Lorentz cone.
// @param b[in/out] The right-hand side vector b before and after adding the
// Lorentz cone.
// @param A_row_count[in/out] The number of rows in A before and after adding
// the Lorentz cone.
// @param lorentz_cone_length[in/out] The length of each Lorentz cone before
// and after adding the Lorentz cone constraint.
void ParseRotatedLorentzConeConstraint(
    const std::vector<Eigen::Triplet<double>>& A_cone_triplets,
    const Eigen::Ref<const Eigen::VectorXd>& b_cone,
    const std::vector<int>& x_indices,
    std::vector<Eigen::Triplet<double>>* A_triplets, std::vector<double>* b,
    int* A_row_count, std::vector<int>* lorentz_cone_length) {
  // Our RotatedLorentzConeConstraint encodes that Ax + b is in the rotated
  // Lorentz cone, namely
  //  (a₀ᵀx + b₀) (a₁ᵀx + b₁) ≥ (a₂ᵀx + b₂)² + ... + (aₙ₋₁ᵀx + bₙ₋₁)²
  //  (a₀ᵀx + b₀) ≥ 0
  //  (a₁ᵀx + b₁) ≥ 0
  // , where aᵢᵀ is the i'th row of A, bᵢ is the i'th row of b. Equivalently the
  // vector
  // [ 0.5(a₀ + a₁)ᵀx + 0.5(b₀ + b₁) ]
  // [ 0.5(a₀ - a₁)ᵀx + 0.5(b₀ - b₁) ]
  // [           a₂ᵀx +           b₂ ]
  //             ...
  // [         aₙ₋₁ᵀx +         bₙ₋₁ ]
  // is in the Lorentz cone. We convert this to the SCS form, that
  //  Cx + s = d
  //  s in Lorentz cone,
  // where C = [ -0.5(a₀ + a₁)ᵀ ]   d = [ 0.5(b₀ + b₁) ]
  //           [ -0.5(a₀ - a₁)ᵀ ]       [ 0.5(b₀ - b₁) ]
  //           [           -a₂ᵀ ]       [           b₂ ]
  //                 ...                      ...
  //           [         -aₙ₋₁ᵀ ]       [          bₙ₋₁]
  for (const auto& Ai_triplet : A_cone_triplets) {
    const int x_index = x_indices[Ai_triplet.col()];
    if (Ai_triplet.row() == 0) {
      A_triplets->emplace_back(*A_row_count, x_index,
                               -0.5 * Ai_triplet.value());
      A_triplets->emplace_back(*A_row_count + 1, x_index,
                               -0.5 * Ai_triplet.value());
    } else if (Ai_triplet.row() == 1) {
      A_triplets->emplace_back(*A_row_count, x_index,
                               -0.5 * Ai_triplet.value());
      A_triplets->emplace_back(*A_row_count + 1, x_index,
                               0.5 * Ai_triplet.value());
    } else {
      A_triplets->emplace_back(*A_row_count + Ai_triplet.row(), x_index,
                               -Ai_triplet.value());
    }
  }
  b->push_back(0.5 * (b_cone(0) + b_cone(1)));
  b->push_back(0.5 * (b_cone(0) - b_cone(1)));
  for (int i = 2; i < b_cone.rows(); ++i) {
    b->push_back(b_cone(i));
  }
  *A_row_count += b_cone.rows();
  lorentz_cone_length->push_back(b_cone.rows());
}

void ParseQuadraticCost(const MathematicalProgram& prog, std::vector<double>* c,
                        std::vector<Eigen::Triplet<double>>* A_triplets,
                        std::vector<double>* b, int* A_row_count,
                        std::vector<int>* lorentz_cone_length, int* num_x) {
  // A QuadraticCost encodes cost of the form
  //   0.5 zᵀQz + pᵀz + r
  // Since SCS only supports linear cost, we introduce a new slack variable y
  // as the upper bound of the cost, with the rotated Lorentz cone constraint
  // 2(y - r - pᵀz) ≥ zᵀQz.
  // We only need to minimize y then.
  for (const auto& cost : prog.quadratic_costs()) {
    // We will convert the expression 2(y - r - pᵀz) ≥ zᵀQz, to the constraint
    // that the vector A_cone * x + b_cone is in the rotated Lorentz cone, where
    // x = [z; y], and A_cone * x + b_cone is
    // [y - r - pᵀz]
    // [          2]
    // [        C*z]
    // where C satisfies Cᵀ*C = Q
    const VectorXDecisionVariable& z = cost.variables();
    const int y_index = z.rows();
    // Ai_triplets are the non-zero entries in the matrix A_cone.
    std::vector<Eigen::Triplet<double>> Ai_triplets;
    Ai_triplets.emplace_back(0, y_index, 1);
    for (int i = 0; i < z.rows(); ++i) {
      Ai_triplets.emplace_back(0, i, -cost.evaluator()->b()(i));
    }
    // Decompose Q to Cᵀ*C
    const Eigen::MatrixXd& Q = cost.evaluator()->Q();
    const Eigen::MatrixXd C =
        math::DecomposePSDmatrixIntoXtransposeTimesX(Q, 1E-10);
    for (int i = 0; i < C.rows(); ++i) {
      for (int j = 0; j < C.cols(); ++j) {
        if (C(i, j) != 0) {
          Ai_triplets.emplace_back(2 + i, j, C(i, j));
        }
      }
    }
    // append the variable y to the end of x
    (*num_x)++;
    std::vector<int> Ai_var_indices = prog.FindDecisionVariableIndices(z);
    Ai_var_indices.push_back(*num_x - 1);
    // Set b_cone
    Eigen::VectorXd b_cone = Eigen::VectorXd::Zero(2 + C.rows());
    b_cone(0) = -cost.evaluator()->c();
    b_cone(1) = 2;
    // Add the rotated Lorentz cone constraint
    ParseRotatedLorentzConeConstraint(Ai_triplets, b_cone, Ai_var_indices,
                                      A_triplets, b, A_row_count,
                                      lorentz_cone_length);
    // Add the cost y.
    c->push_back(1);
  }
}

void ParseLinearConstraint(const MathematicalProgram& prog,
                           std::vector<Eigen::Triplet<double>>* A_triplets,
                           std::vector<double>* b, int* A_row_count,
                           ScsCone* cone) {
  // The linear constraint lb ≤ aᵀx ≤ ub is converted to
  //  aᵀx + s1 = ub,
  // -aᵀx + s2 = lb
  // s1, s2 in the positive cone.
  // The special cases are when ub = ∞ or lb = -∞.
  // When ub = ∞, then we only add the constraint
  // -aᵀx + s = lb, s in the positive cone.
  // When lb = -∞, then we only add the constraint
  // aᵀx + s = ub, s in the positive cone.
  int num_linear_constraint_rows = 0;
  for (const auto& linear_constraint : prog.linear_constraints()) {
    const Eigen::VectorXd& ub = linear_constraint.evaluator()->upper_bound();
    const Eigen::VectorXd& lb = linear_constraint.evaluator()->lower_bound();
    const VectorXDecisionVariable& x = linear_constraint.variables();
    const Eigen::MatrixXd& Ai = linear_constraint.evaluator()->A();
    for (int i = 0; i < linear_constraint.evaluator()->num_constraints();
         ++i) {
      const bool is_ub_finite{!std::isinf(ub(i))};
      const bool is_lb_finite{!std::isinf(lb(i))};
      if (is_ub_finite || is_lb_finite) {
        // If lb != -∞, then the constraint -aᵀx + s = lb will be added to the
        // matrix A, in the row lower_bound_row_index.
        const int lower_bound_row_index =
            *A_row_count + num_linear_constraint_rows;
        // If ub != ∞, then the constraint aᵀx + s = ub will be added to the
        // matrix A, in the row upper_bound_row_index.
        const int upper_bound_row_index =
            *A_row_count + num_linear_constraint_rows + (is_lb_finite ? 1 : 0);
        for (int j = 0; j < x.rows(); ++j) {
          if (Ai(i, j) != 0) {
            const int xj_index = prog.FindDecisionVariableIndex(x(j));
            if (is_ub_finite) {
              A_triplets->emplace_back(upper_bound_row_index, xj_index,
                                       Ai(i, j));
            }
            if (is_lb_finite) {
              A_triplets->emplace_back(lower_bound_row_index, xj_index,
                                       -Ai(i, j));
            }
          }
        }
        if (is_lb_finite) {
          b->push_back(-lb(i));
          ++num_linear_constraint_rows;
        }
        if (is_ub_finite) {
          b->push_back(ub(i));
          ++num_linear_constraint_rows;
        }
      }
    }
  }
  *A_row_count += num_linear_constraint_rows;
  cone->l += num_linear_constraint_rows;
}

void ParseLinearEqualityConstraint(
    const MathematicalProgram& prog,
    std::vector<Eigen::Triplet<double>>* A_triplets, std::vector<double>* b,
    int* A_row_count, ScsCone* cone) {
  int num_linear_equality_constraints_rows = 0;
  // The linear equality constraint A x = b is converted to
  // A x + s = b. s in zero cone.
  for (const auto& linear_equality_constraint :
       prog.linear_equality_constraints()) {
    const Eigen::SparseMatrix<double> Ai =
        linear_equality_constraint.evaluator()->GetSparseMatrix();
    const std::vector<Eigen::Triplet<double>> Ai_triplets =
        math::SparseMatrixToTriplets(Ai);
    A_triplets->reserve(A_triplets->size() + Ai_triplets.size());
    const solvers::VectorXDecisionVariable& x =
        linear_equality_constraint.variables();
    // x_indices[i] is the index of x(i)
    const std::vector<int> x_indices = prog.FindDecisionVariableIndices(x);
    for (const auto& Ai_triplet : Ai_triplets) {
      A_triplets->emplace_back(Ai_triplet.row() + *A_row_count,
                               x_indices[Ai_triplet.col()], Ai_triplet.value());
    }
    const int num_Ai_rows =
        linear_equality_constraint.evaluator()->num_constraints();
    b->reserve(b->size() + num_Ai_rows);
    for (int i = 0; i < num_Ai_rows; ++i) {
      b->push_back(linear_equality_constraint.evaluator()->lower_bound()(i));
    }
    *A_row_count += num_Ai_rows;
    num_linear_equality_constraints_rows += num_Ai_rows;
  }
  cone->f += num_linear_equality_constraints_rows;
}

void ParseBoundingBoxConstraint(const MathematicalProgram& prog,
                                std::vector<Eigen::Triplet<double>>* A_triplets,
                                std::vector<double>* b, int* A_row_count,
                                ScsCone* cone) {
  // A bounding box constraint lb ≤ x ≤ ub is converted to the SCS form as
  // x + s1 = ub, -x + s2 = -lb, s1, s2 in the positive cone.
  // TODO(hongkai.dai) : handle the special case l = u, such that we can convert
  // it to x + s = l, s in zero cone.
  int num_bounding_box_constraint_rows = 0;
  for (const auto& bounding_box_constraint : prog.bounding_box_constraints()) {
    int num_scs_new_constraint = 0;
    const VectorXDecisionVariable& xi = bounding_box_constraint.variables();
    const int num_xi_rows = xi.rows();
    A_triplets->reserve(A_triplets->size() + 2 * num_xi_rows);
    b->reserve(b->size() + 2 * num_xi_rows);
    for (int i = 0; i < num_xi_rows; ++i) {
      if (!std::isinf(bounding_box_constraint.evaluator()->upper_bound()(i))) {
        // if ub != ∞, then add the constraint x + s1 = ub, s1 in the positive
        // cone.
        A_triplets->emplace_back(num_scs_new_constraint + *A_row_count,
                                 prog.FindDecisionVariableIndex(xi(i)), 1);
        b->push_back(bounding_box_constraint.evaluator()->upper_bound()(i));
        ++num_scs_new_constraint;
      }
      if (!std::isinf(bounding_box_constraint.evaluator()->lower_bound()(i))) {
        // if lb != -∞, then add the constraint -x + s2 = -lb, s2 in the
        // positive cone.
        A_triplets->emplace_back(num_scs_new_constraint + *A_row_count,
                                 prog.FindDecisionVariableIndex(xi(i)), -1);
        b->push_back(-bounding_box_constraint.evaluator()->lower_bound()(i));
        ++num_scs_new_constraint;
      }
    }
    *A_row_count += num_scs_new_constraint;
    num_bounding_box_constraint_rows += num_scs_new_constraint;
  }
  cone->l += num_bounding_box_constraint_rows;
}

void ParseSecondOrderConeConstraints(
    const MathematicalProgram& prog,
    std::vector<Eigen::Triplet<double>>* A_triplets, std::vector<double>* b,
    int* A_row_count, std::vector<int>* lorentz_cone_length) {
  // Our LorentzConeConstraint encodes that Ax + b is in Lorentz cone. We
  // convert this to SCS form, as -Ax + s = b, s in Lorentz cone.
  for (const auto& lorentz_cone_constraint : prog.lorentz_cone_constraints()) {
    // x_indices[i] is the index of x(i)
    const VectorXDecisionVariable& x = lorentz_cone_constraint.variables();
    const std::vector<int> x_indices = prog.FindDecisionVariableIndices(x);
    const Eigen::SparseMatrix<double> Ai =
        lorentz_cone_constraint.evaluator()->A();
    const std::vector<Eigen::Triplet<double>> Ai_triplets =
        math::SparseMatrixToTriplets(Ai);
    for (const auto& Ai_triplet : Ai_triplets) {
      A_triplets->emplace_back(Ai_triplet.row() + *A_row_count,
                               x_indices[Ai_triplet.col()],
                               -Ai_triplet.value());
    }
    const int num_Ai_rows = lorentz_cone_constraint.evaluator()->A().rows();
    for (int i = 0; i < num_Ai_rows; ++i) {
      b->push_back(lorentz_cone_constraint.evaluator()->b()(i));
    }
    lorentz_cone_length->push_back(num_Ai_rows);
    *A_row_count += num_Ai_rows;
  }

  for (const auto& rotated_lorentz_cone :
       prog.rotated_lorentz_cone_constraints()) {
    const VectorXDecisionVariable& x = rotated_lorentz_cone.variables();
    const std::vector<int> x_indices = prog.FindDecisionVariableIndices(x);
    const Eigen::SparseMatrix<double> Ai =
        rotated_lorentz_cone.evaluator()->A();
    const Eigen::VectorXd& bi = rotated_lorentz_cone.evaluator()->b();
    const std::vector<Eigen::Triplet<double>> Ai_triplets =
        math::SparseMatrixToTriplets(Ai);
    ParseRotatedLorentzConeConstraint(Ai_triplets, bi, x_indices, A_triplets, b,
                                      A_row_count, lorentz_cone_length);
  }
}

// This function parses both PositiveSemidefinite and
// LinearMatrixInequalityConstraint.
void ParsePositiveSemidefiniteConstraint(
    const MathematicalProgram& prog,
    std::vector<Eigen::Triplet<double>>* A_triplets, std::vector<double>* b,
    int* A_row_count, ScsCone* cone) {
  std::vector<int> psd_cone_length;
  const double sqrt2 = std::sqrt(2);
  for (const auto& psd_constraint : prog.positive_semidefinite_constraints()) {
    // PositiveSemidefiniteConstraint encodes the matrix X being psd.
    // We convert it to SCS form
    // A * x + s = 0
    // s in positive semidefinite cone.
    // where A is a diagonal matrix, with its diagonal entries being the stacked
    // column vector of the lower triangular part of matrix
    // ⎡ -1 -√2 -√2 ... -√2⎤
    // ⎢-√2  -1 -√2 ... -√2⎥
    // ⎢-√2 -√2  -1 ... -√2⎥
    // ⎢    ...            ⎥
    // ⎣-√2 -√2 -√2 ...  -1⎦
    // The √2 scaling factor in the off-diagonal entries are required by SCS,
    // as it uses only the lower triangular part of the symmetric matrix, as
    // explained in https://github.com/cvxgrp/scs
    // x is the stacked column vector of the lower triangular part of the
    // symmetric matrix X.
    const int X_rows = psd_constraint.evaluator()->matrix_rows();
    int x_index_count = 0;
    const VectorXDecisionVariable& flat_X = psd_constraint.variables();
    DRAKE_DEMAND(flat_X.rows() == X_rows * X_rows);
    b->reserve(b->size() + X_rows * (X_rows + 1) / 2);
    for (int j = 0; j < X_rows; ++j) {
      A_triplets->emplace_back(
          *A_row_count + x_index_count,
          prog.FindDecisionVariableIndex(flat_X(j * X_rows + j)), -1);
      b->push_back(0);
      ++x_index_count;
      for (int i = j + 1; i < X_rows; ++i) {
        A_triplets->emplace_back(
            *A_row_count + x_index_count,
            prog.FindDecisionVariableIndex(flat_X(j * X_rows + i)), -sqrt2);
        b->push_back(0);
        ++x_index_count;
      }
    }
    (*A_row_count) += X_rows * (X_rows + 1) / 2;
    psd_cone_length.push_back(X_rows);
  }
  for (const auto& lmi_constraint :
       prog.linear_matrix_inequality_constraints()) {
    // LinearMatrixInequalityConstraint encodes
    // F₀ + x₁*F₁ + x₂*F₂ + ... + xₙFₙ is p.s.d
    // We convert this to SCS form as
    // A_cone * x + s = b_cone
    // s in SCS positive semidefinite cone.
    // where
    //              ⎡  F₁(0, 0)   F₂(0, 0) ...   Fₙ(0, 0)⎤
    //              ⎢√2F₁(1, 0) √2F₂(1, 0) ... √2Fₙ(1, 0)⎥
    //   A_cone = - ⎢√2F₁(2, 0) √2F₂(2, 0) ... √2Fₙ(2, 0)⎥,
    //              ⎢            ...                     ⎥
    //              ⎣  F₁(m, m)   F₂(m, m) ...   Fₙ(m, m)⎦
    //
    //              ⎡  F₀(0, 0)⎤
    //              ⎢√2F₀(1, 0)⎥
    //   b_cone =   ⎢√2F₀(2, 0)⎥,
    //              ⎢   ...    ⎥
    //              ⎣  F₀(m, m)⎦
    // and
    //   x = [x₁; x₂; ... ; xₙ].
    // As explained above, the off-diagonal rows are scaled by √2. Please refer
    // to https://github.com/cvxgrp/scs about the scaling factor √2.
    const std::vector<Eigen::MatrixXd>& F = lmi_constraint.evaluator()->F();
    const VectorXDecisionVariable& x = lmi_constraint.variables();
    const int F_rows = lmi_constraint.evaluator()->matrix_rows();
    const std::vector<int> x_indices = prog.FindDecisionVariableIndices(x);
    int A_cone_row_count = 0;
    b->reserve(b->size() + F_rows * (F_rows + 1) / 2);
    for (int j = 0; j < F_rows; ++j) {
      for (int i = j; i < F_rows; ++i) {
        const double scale_factor = i == j ? 1 : sqrt2;
        for (int k = 1; k < static_cast<int>(F.size()); ++k) {
          A_triplets->emplace_back(*A_row_count + A_cone_row_count,
                                   x_indices[k - 1],
                                   -scale_factor * F[k](i, j));
        }
        b->push_back(scale_factor * F[0](i, j));
        ++A_cone_row_count;
      }
    }
    *A_row_count += F_rows * (F_rows + 1) / 2;
    psd_cone_length.push_back(F_rows);
  }
  cone->ssize = psd_cone_length.size();
  cone->s = static_cast<scs_int*>(scs_calloc(cone->ssize, sizeof(scs_int)));
  for (int i = 0; i < cone->ssize; ++i) {
    cone->s[i] = psd_cone_length[i];
  }
}

void ParseExponentialConeConstraint(
    const MathematicalProgram& prog,
    std::vector<Eigen::Triplet<double>>* A_triplets, std::vector<double>* b,
    int* A_row_count, ScsCone* cone) {
  cone->ep = static_cast<int>(prog.exponential_cone_constraints().size());
  for (const auto& binding : prog.exponential_cone_constraints()) {
    // drake::solvers::ExponentialConstraint enforces that z = A * x + b is in
    // the exponential cone (z₀ ≥ z₁*exp(z₂ / z₁)). This is different from the
    // exponential cone used in SCS. In SCS, a vector s is in the exponential
    // cone, if s₂≥ s₁*exp(s₀ / s₁). To transform drake's Exponential cone to
    // SCS's exponential cone, we use
    // -[A.row(2); A.row(1); A.row(0)] * x + s = [b(2); b(1); b(0)], and s is
    // in SCS's exponential cone, where A = binding.evaluator()->A(), b =
    // binding.evaluator()->b().
    const int num_bound_variables = binding.variables().rows();
    for (int i = 0; i < num_bound_variables; ++i) {
      A_triplets->reserve(A_triplets->size() +
                          binding.evaluator()->A().nonZeros());
      const int decision_variable_index =
          prog.FindDecisionVariableIndex(binding.variables()(i));
      for (Eigen::SparseMatrix<double>::InnerIterator it(
               binding.evaluator()->A(), i);
           it; ++it) {
        // 2 - it.row() is used for reverse the row order, as mentioned in the
        // function documentation above.
        A_triplets->emplace_back(*A_row_count + 2 - it.row(),
                                 decision_variable_index, -it.value());
      }
    }
    // The exponential cone constraint is on a 3 x 1 vector, hence for each
    // ExponentialConeConstraint, we append 3 rows to A and b.
    b->reserve(b->size() + 3);
    for (int i = 0; i < 3; ++i) {
      b->push_back(binding.evaluator()->b()(2 - i));
    }
    *A_row_count += 3;
  }
}

std::string Scs_return_info(scs_int scs_status) {
  switch (scs_status) {
    case SCS_INFEASIBLE_INACCURATE:
      return "SCS infeasible inaccurate";
    case SCS_UNBOUNDED_INACCURATE:
      return "SCS unbounded inaccurate";
    case SCS_SIGINT:
      return "SCS sigint";
    case SCS_FAILED:
      return "SCS failed";
    case SCS_INDETERMINATE:
      return "SCS indeterminate";
    case SCS_INFEASIBLE:
      return "SCS primal infeasible, dual unbounded";
    case SCS_UNBOUNDED:
      return "SCS primal unbounded, dual infeasible";
    case SCS_UNFINISHED:
      return "SCS unfinished";
    case SCS_SOLVED:
      return "SCS solved";
    case SCS_SOLVED_INACCURATE:
      return "SCS solved inaccurate";
    default:
      throw std::runtime_error("Unknown scs status.");
  }
}

void SetScsProblemData(int A_row_count, int num_vars,
                       const Eigen::SparseMatrix<double>& A,
                       const std::vector<double>& b,
                       const std::vector<double>& c,
                       ScsData* scs_problem_data) {
  scs_problem_data->m = A_row_count;
  scs_problem_data->n = num_vars;

  scs_problem_data->A = static_cast<ScsMatrix*>(malloc(sizeof(ScsMatrix)));
  scs_problem_data->A->x =
      static_cast<scs_float*>(scs_calloc(A.nonZeros(), sizeof(scs_float)));

  scs_problem_data->A->i =
      static_cast<scs_int*>(scs_calloc(A.nonZeros(), sizeof(scs_int)));

  scs_problem_data->A->p = static_cast<scs_int*>(
      scs_calloc(scs_problem_data->n + 1, sizeof(scs_int)));

  // TODO(hongkai.dai): should I use memcpy for the assignment in the for loop?
  for (int i = 0; i < A.nonZeros(); ++i) {
    scs_problem_data->A->x[i] = *(A.valuePtr() + i);
    scs_problem_data->A->i[i] = *(A.innerIndexPtr() + i);
  }
  for (int i = 0; i < scs_problem_data->n + 1; ++i) {
    scs_problem_data->A->p[i] = *(A.outerIndexPtr() + i);
  }
  scs_problem_data->A->m = scs_problem_data->m;
  scs_problem_data->A->n = scs_problem_data->n;

  scs_problem_data->b =
      static_cast<scs_float*>(scs_calloc(b.size(), sizeof(scs_float)));

  for (int i = 0; i < static_cast<int>(b.size()); ++i) {
    scs_problem_data->b[i] = b[i];
  }
  scs_problem_data->c =
      static_cast<scs_float*>(scs_calloc(num_vars, sizeof(scs_float)));
  for (int i = 0; i < num_vars; ++i) {
    scs_problem_data->c[i] = c[i];
  }

  // Set the parameters to default values.
  scs_problem_data->stgs =
      static_cast<ScsSettings*>(scs_calloc(1, sizeof(ScsSettings)));
  scs_set_default_settings(scs_problem_data);
}
}  // namespace

bool ScsSolver::is_available() { return true; }

namespace {
void SetScsSettings(std::unordered_map<std::string, int>* solver_options_int,
                    SCS_SETTINGS* scs_settings) {
  auto it = solver_options_int->find("max_iters");
  if (it != solver_options_int->end()) {
    scs_settings->max_iters = it->second;
    solver_options_int->erase(it);
  }
  it = solver_options_int->find("normalize");
  if (it != solver_options_int->end()) {
    scs_settings->normalize = it->second;
    solver_options_int->erase(it);
  }
  it = solver_options_int->find("verbose");
  if (it != solver_options_int->end()) {
    scs_settings->verbose = it->second;
    solver_options_int->erase(it);
  }
  it = solver_options_int->find("warm_start");
  if (it != solver_options_int->end()) {
    scs_settings->warm_start = it->second;
    solver_options_int->erase(it);
  }
  it = solver_options_int->find("acceleration_lookback");
  if (it != solver_options_int->end()) {
    scs_settings->acceleration_lookback = it->second;
    solver_options_int->erase(it);
  }
  if (!solver_options_int->empty()) {
    throw std::invalid_argument("Unsupported SCS solver options.");
  }
}

void SetScsSettings(
    std::unordered_map<std::string, double>* solver_options_double,
    SCS_SETTINGS* scs_settings) {
  auto it = solver_options_double->find("scale");
  if (it != solver_options_double->end()) {
    scs_settings->scale = it->second;
    solver_options_double->erase(it);
  }
  it = solver_options_double->find("rho_x");
  if (it != solver_options_double->end()) {
    scs_settings->rho_x = it->second;
    solver_options_double->erase(it);
  }
  it = solver_options_double->find("eps");
  if (it != solver_options_double->end()) {
    scs_settings->eps = it->second;
    solver_options_double->erase(it);
  }
  it = solver_options_double->find("alpha");
  if (it != solver_options_double->end()) {
    scs_settings->alpha = it->second;
    solver_options_double->erase(it);
  }
  it = solver_options_double->find("cg_rate");
  if (it != solver_options_double->end()) {
    scs_settings->cg_rate = it->second;
    solver_options_double->erase(it);
  }
  if (!solver_options_double->empty()) {
    throw std::invalid_argument("Unsupported SCS solver options.");
  }
}
}  // namespace

void ScsSolver::DoSolve(
    const MathematicalProgram& prog,
    const Eigen::VectorXd& initial_guess,
    const SolverOptions& merged_options,
    MathematicalProgramResult* result) const {
  if (!prog.GetVariableScaling().empty()) {
    static const logging::Warn log_once(
      "ScsSolver doesn't support the feature of variable scaling.");
  }

  // TODO(hongkai.dai): allow warm starting SCS with initial guess on
  // primal/dual variables and primal residues.
  unused(initial_guess);
  // The initial guess for SCS is unused.
  // SCS solves the problem in this form
  // min  cᵀx
  // s.t A x + s = b
  //     s in K
  // where K is a Cartesian product of some primitive cones.
  // The cones have to be in this order
  // Zero cone {x | x = 0 }
  // Positive orthant {x | x ≥ 0 }
  // Second-order cone {(t, x) | |x|₂ ≤ t }
  // Positive semidefinite cone { X | min(eig(X)) ≥ 0, X = Xᵀ }
  // There are more types that are supported by SCS, after the Positive
  // semidefinite cone. Please refer to https://github.com/cvxgrp/scs for more
  // details on the cone types.
  // Notice that due to the special problem form supported by SCS, we need to
  // convert our generic constraints to SCS form. For example, a linear
  // inequality constraint
  //   A x ≤ b
  // will be converted as
  //   A x + s = b
  //   s in positive orthant cone.
  // by introducing the slack variable s.

  // num_x is the number of variables in `x`. It can contain more variables
  // than in prog. For some constraint/cost, we need to append slack variables
  // to convert the constraint/cost to the SCS form. For example, SCS does not
  // support quadratic cost. So we need to convert the quadratic cost
  //   min 0.5zᵀQz + bᵀz + d
  // to the form
  //   min y
  //   s.t 2(y - bᵀz - d) ≥ zᵀQz     (1)
  // now the cost is a linear function of y, with a rotated Lorentz cone
  // constraint(1). So we need to append the slack variable `y` to the variables
  // in `prog`.
  int num_x = prog.num_vars();

  // We need to construct a sparse matrix in Column Compressed Storage (CCS)
  // format. On the other hand, we add the constraint row by row, instead of
  // column by column, as preferred by CCS. As a result, we use Eigen sparse
  // matrix to construct a sparse matrix in column order first, and then
  // compress it to get CCS.
  std::vector<Eigen::Triplet<double>> A_triplets;

  // cone stores all the cones K in the problem.
  ScsCone* cone = static_cast<ScsCone*>(scs_calloc(1, sizeof(ScsCone)));

  // A_row_count will increment, when we add each constraint.
  int A_row_count = 0;
  std::vector<double> b;

  // `c` is the coefficient in the linear cost cᵀx
  std::vector<double> c(num_x, 0.0);

  // Our cost (LinearCost, QuadraticCost, etc) also allows a constant term, we
  // add these constant terms to `cost_constant`.
  double cost_constant{0};

  // Parse linear cost
  ParseLinearCost(prog, &c, &cost_constant);

  // Parse linear equality constraint
  ParseLinearEqualityConstraint(prog, &A_triplets, &b, &A_row_count, cone);

  // Parse bounding box constraint
  ParseBoundingBoxConstraint(prog, &A_triplets, &b, &A_row_count, cone);

  // Parse linear constraint
  ParseLinearConstraint(prog, &A_triplets, &b, &A_row_count, cone);

  // Parse Lorentz cone and rotated Lorentz cone constraint
  std::vector<int> lorentz_cone_length;
  ParseSecondOrderConeConstraints(prog, &A_triplets, &b, &A_row_count,
                                  &lorentz_cone_length);

  // Parse quadratic cost. This MUST be called after parsing the second order
  // cone constraint, as we convert quadratic cost to second order cone
  // constraint.
  ParseQuadraticCost(prog, &c, &A_triplets, &b, &A_row_count,
                     &lorentz_cone_length, &num_x);

  // Set the lorentz cone length in the SCS cone.
  cone->qsize = lorentz_cone_length.size();
  cone->q = static_cast<scs_int*>(scs_calloc(cone->qsize, sizeof(scs_int)));
  for (int i = 0; i < cone->qsize; ++i) {
    cone->q[i] = lorentz_cone_length[i];
  }

  // Parse PositiveSemidefiniteConstraint and LinearMatrixInequalityConstraint.
  ParsePositiveSemidefiniteConstraint(prog, &A_triplets, &b, &A_row_count,
                                      cone);

  // Parse ExponentialConeConstraint.
  ParseExponentialConeConstraint(prog, &A_triplets, &b, &A_row_count, cone);

  Eigen::SparseMatrix<double> A(A_row_count, num_x);
  A.setFromTriplets(A_triplets.begin(), A_triplets.end());
  A.makeCompressed();

  ScsData* scs_problem_data =
      static_cast<ScsData*>(scs_calloc(1, sizeof(ScsData)));
  SetScsProblemData(A_row_count, num_x, A, b, c, scs_problem_data);
  std::unordered_map<std::string, int> input_solver_options_int =
      merged_options.GetOptionsInt(id());
  std::unordered_map<std::string, double> input_solver_options_double =
      merged_options.GetOptionsDouble(id());
  SetScsSettings(&input_solver_options_int, scs_problem_data->stgs);
  SetScsSettings(&input_solver_options_double, scs_problem_data->stgs);

  ScsInfo scs_info{0};

  ScsSolution* scs_sol =
      static_cast<ScsSolution*>(scs_calloc(1, sizeof(ScsSolution)));

  ScsSolverDetails& solver_details =
      result->SetSolverDetailsType<ScsSolverDetails>();
  solver_details.scs_status = scs(scs_problem_data, cone, scs_sol, &scs_info);

  solver_details.iter = scs_info.iter;
  solver_details.primal_objective = scs_info.pobj;
  solver_details.dual_objective = scs_info.dobj;
  solver_details.primal_residue = scs_info.res_pri;
  solver_details.residue_infeasibility = scs_info.res_infeas;
  solver_details.residue_unbounded = scs_info.res_unbdd;
  solver_details.relative_duality_gap = scs_info.rel_gap;
  solver_details.scs_setup_time = scs_info.setup_time;
  solver_details.scs_solve_time = scs_info.solve_time;

  SolutionResult solution_result{SolutionResult::kUnknownError};
  solver_details.y.resize(A_row_count);
  solver_details.s.resize(A_row_count);
  for (int i = 0; i < A_row_count; ++i) {
    solver_details.y(i) = scs_sol->y[i];
    solver_details.s(i) = scs_sol->s[i];
  }
  if (solver_details.scs_status == SCS_SOLVED ||
      solver_details.scs_status == SCS_SOLVED_INACCURATE) {
    solution_result = SolutionResult::kSolutionFound;
    result->set_x_val(
        (Eigen::Map<VectorX<scs_float>>(scs_sol->x, prog.num_vars()))
            .cast<double>());
    result->set_optimal_cost(scs_info.pobj + cost_constant);
  } else if (solver_details.scs_status == SCS_UNBOUNDED ||
             solver_details.scs_status == SCS_UNBOUNDED_INACCURATE) {
    solution_result = SolutionResult::kUnbounded;
    result->set_optimal_cost(MathematicalProgram::kUnboundedCost);
  } else if (solver_details.scs_status == SCS_INFEASIBLE ||
             solver_details.scs_status == SCS_INFEASIBLE_INACCURATE) {
    solution_result = SolutionResult::kInfeasibleConstraints;
    result->set_optimal_cost(MathematicalProgram::kGlobalInfeasibleCost);
  }
  if (solver_details.scs_status != SCS_SOLVED) {
    drake::log()->info("SCS returns code {}, with message \"{}\".\n",
                       solver_details.scs_status,
                       Scs_return_info(solver_details.scs_status));
  }

  result->set_solution_result(solution_result);

  // Free allocated memory
  scs_free_data(scs_problem_data, cone);
  scs_free_sol(scs_sol);
}

}  // namespace solvers
}  // namespace drake


/********************************************************************************
 *
 * This file is part of the Geneva library collection. The following license
 * applies to this file:
 *
 * ------------------------------------------------------------------------------
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 * ------------------------------------------------------------------------------
 *
 * Note that other files in the Geneva library collection may use a different
 * license. Please see the licensing information in each file.
 *
 ********************************************************************************
 *
 * Geneva was started by Dr. Rüdiger Berlich and was later maintained together
 * with Dr. Ariel Garcia under the auspices of Gemfony scientific. For further
 * information on Gemfony scientific, see http://www.gemfomy.eu .
 *
 * The majority of files in Geneva was released under the Apache license v2.0
 * in February 2020.
 *
 * See the NOTICE file in the top-level directory of the Geneva library
 * collection for a list of contributors and copyright information.
 *
 ********************************************************************************/

#pragma once

// Global checks, defines and includes needed for all of Geneva
#include "common/GGlobalDefines.hpp"

// Standard headers go here
#include <vector>
#include <sstream>
#include <iostream>
#include <string>
#include <cstdlib>
#include <cmath>
#include <fstream>
#include <typeinfo>
#include <tuple>
#include <limits>

// Boost headers go here
#include <boost/filesystem.hpp>
#include <boost/math/special_functions.hpp>
#include <boost/numeric/conversion/bounds.hpp>

// Geneva headers go here
#include "common/GCommonEnums.hpp"
#include "common/GExceptions.hpp"
#include "common/GLogger.hpp"
#include "common/GErrorStreamer.hpp"
#include "common/GCommonMathHelperFunctions.hpp"

namespace Gem {
namespace Common {

const bool GWARNINGONLY = true;
const bool GERRORONLY = false;

/******************************************************************************/
/**
 * Enforces a value inside of a given range (both boundaries inclusive) for the
 * first parameter. Note that the value of this parameter may change.
 *
 * @param val The value to be adapted
 * @param lower The lower boundary of the allowed value range
 * @param upper The upper (inclusive) boundary of the allowed value range
 */
template<typename fp_type>
fp_type enforceRangeConstraint(
	fp_type &val
	, const fp_type &lower
	, const fp_type &upper
	, const std::string& caller = "empty"
	, bool verbose = false
	, typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
   if(lower > upper) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< (caller=="empty"?"":("["+caller+"] ")) << "In enforceRangeConstraint<fp_type>(): Error!" << std::endl
				<< "Lower boundary > upper boundary: " << lower << " / " << upper << std::endl
		);
   }

	if (val < lower) {
		if(verbose) {
			glogger
				<< (caller == "empty" ? "" : ("[" + caller + "] ")) << "In Gem::Common::enforceRangeConstraint(): " << std::endl
				<< "value " << val << " < lower boundary " << lower << std::endl
				<< "Will be adapted to " << lower << std::endl
				<< GWARNING;
		}
		val = lower;
	} else if (val > upper) {
		if(verbose) {
			glogger
				<< (caller == "empty" ? "" : ("[" + caller + "] ")) << "In Gem::Common::enforceRangeConstraint(): " << std::endl
			 	<< "value " << val << " > upper boundary " << upper << std::endl
				<< "Will be adapted to " << upper << std::endl
				<< GWARNING;
		}
		val = upper;
	}

	return val;
}

/******************************************************************************/
/**
 * Checks that a given floating point value is inside of a given set of boundaries (both inclusive)
 *
 * @param val The value to be check
 * @param lower The lower boundary of the allowed value range
 * @param upper The upper boundary of the allowed value range
 */
template<typename fp_type>
bool checkRangeCompliance(
	const fp_type &val
	, const fp_type &lower
	, const fp_type &upper
	, const std::string& caller = "empty"
	, typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
   if(lower > upper) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< (caller=="empty"?"":("["+caller+"] ")) << "In checkRangeCompliance<fp_type>(...): Error!" << std::endl
				<< "Lower boundary > upper boundary: " << lower << " / " << upper << std::endl
		);
   }

	return not (val < lower || val > upper);
}

/******************************************************************************/
/**
 * Checks that a given floating point value is inside of a given set of boundaries (both inclusive)
 *
 * @param val The value to be check
 * @param lower The lower boundary of the allowed value range
 * @param upper The upper boundary of the allowed value range
 */
template<typename int_type>
bool checkRangeCompliance(
	const int_type &val
	, const int_type &lower
	, const int_type &upper
	, const std::string& caller = "empty"
	, typename std::enable_if<std::is_integral<int_type>::value>::type *dummy = nullptr
) {
	if(lower > upper) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< (caller=="empty"?"":("["+caller+"] ")) << "In checkRangeCompliance<int_type>(...): Error!" << std::endl
				<< "Lower boundary > upper boundary: " << lower << " / " << upper << std::endl
		);
	}

	return not (val < lower || val > upper);
}

/******************************************************************************/
/**
 * Retrieves the worst known value for a given floating point type, depending
 * on whether maximal or minimal values are considered to be better
 */
template<typename fp_type>
fp_type getWorstCase(
	bool maxMode
	, typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
	return (
		maxMode
		? boost::numeric::bounds<fp_type>::lowest()
		: boost::numeric::bounds<fp_type>::highest()
	);
}

/******************************************************************************/
/**
 * Retrieves the best known value for a given floating point type, depending
 * on whether maximal or minimal values are considered to be better
 */
template<typename fp_type>
fp_type getBestCase(
	bool maxMode
	, typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
	return (
		maxMode
		? boost::numeric::bounds<fp_type>::highest()
		: boost::numeric::bounds<fp_type>::lowest()
	);
}

/******************************************************************************/
/**
 * Retrieves the worst known value for a given floating point type, depending
 * on whether maximal or minimal values are considered to be better
 */
template<typename fp_type>
fp_type getWorstCase(
	Gem::Common::sortOrder sortOrder
	, typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
	return (
		sortOrder==Gem::Common::sortOrder::HIGHERISBETTER
		? boost::numeric::bounds<fp_type>::lowest()
		: boost::numeric::bounds<fp_type>::highest()
	);
}

/******************************************************************************/
/**
 * Retrieves the best known value for a given floating point type, depending
 * on whether maximal or minimal values are considered to be better
 */
template<typename fp_type>
fp_type getBestCase(
	Gem::Common::sortOrder sortOrder
	, typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
	return (
		sortOrder==Gem::Common::sortOrder::HIGHERISBETTER
		? boost::numeric::bounds<fp_type>::highest()
		: boost::numeric::bounds<fp_type>::lowest()
	);
}

/******************************************************************************/
/**
 * Checks that a floating point value is contained in a given range
 *
 * @param val The value to be checked for containment
 * @param min The lower boundary (included)
 * @param max The upper boundary (possibly included)
 * @param lowerOpen Determines whether the lower boundary must be smaller or may be equal to val (default: closed)
 * @param upperOpen Determines whether the upper boundary must be larger or may be equal to val (default: closed)
 * @param warnOnly Will warn only if the condition isn't met
 * @return The value being checked
 */
const bool GFPLOWERCLOSED = false;
const bool GFPLOWEROPEN = true;
const bool GFPUPPERCLOSED = false;
const bool GFPUPPEROPEN = true;
const bool GFWARNONLY = true;
const bool GFNOWARNING = false;

template<typename fp_type>
fp_type checkValueRange(
	fp_type val
	, fp_type min
	, fp_type max
	, bool lowerOpen = false
	, bool upperOpen = false
	, bool warnOnly = false
	, std::string varName = std::string()
	, typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
	bool inValueRange = true;

	if (lowerOpen) {
		if (val < boost::math::float_next<fp_type>(min)) inValueRange = false;
	} else {
		if (val < min) inValueRange = false;
	}

	if (upperOpen) {
		if (val > boost::math::float_prior<fp_type>(max)) inValueRange = false;
	} else {
		if (val > max) inValueRange = false;
	}

	if (not inValueRange) {
		if (warnOnly) {
			glogger
			<< "In checkValueRange<fp_type>(): Error!" << std::endl
			<< "Value " << val << (varName.empty() ? "" : (" of variable " + varName)) << " outside of recommended range " << std::endl
			<< min << (lowerOpen ? " (open) - " : " (closed) - ") << max << (upperOpen ? " (open)" : " (closed)") << std::endl
			<< GWARNING;
		} else {
			throw gemfony_exception(
				g_error_streamer(DO_LOG, time_and_place)
					<< "In checkValueRange<fp_type>(): Error!" << std::endl
					<< "Value " << val << (varName.empty() ? "" : (" of variable " + varName)) << " outside of allowed range " << std::endl
					<< min << (lowerOpen ? " (open) - " : " (closed) - ") << max << (upperOpen ? " (open)" : " (closed)") << std::endl
			);
		}
	}

	return val;
}

/******************************************************************************/
/**
 * Checks that an integral value is contained in a given range
 *
 * @param val The value to be checked for containment
 * @param min The lower boundary (included)
 * @param max The upper boundary (possibly included)
 * @param lowerOpen Determines whether the lower boundary must be smaller or may be equal to val (default: closed)
 * @param upperOpen Determines whether the upper boundary must be larger or may be equal to val (default: closed)
 * @param warnOnly Will warn only if the condition isn't met
 * @return The value being checked
 */
const bool GINTLOWERCLOSED = false;
const bool GINTLOWEROPEN = true;
const bool GINTUPPERCLOSED = false;
const bool GINTUPPEROPEN = true;

template<typename int_type>
int_type checkValueRange(
	int_type val, int_type min, int_type max, bool lowerOpen = false, bool upperOpen = false, bool warnOnly = false,
	typename std::enable_if<std::is_integral<int_type>::value>::type *dummy = nullptr
) {
	bool inValueRange = true;

	if (lowerOpen) {
		if (val <= min) inValueRange = false;
	} else {
		if (val < min) inValueRange = false;
	}

	if (upperOpen) {
		if (val >= max) inValueRange = false;
	} else {
		if (val > max) inValueRange = false;
	}

	if (not inValueRange) {
		if (warnOnly) {
			glogger
			<< "Warning:" << std::endl
			<< "In checkValueRange<int_type>(): Error!" << std::endl
			<< "Value " << val << " outside of recommended range " << std::endl
			<< min << (lowerOpen ? " (open) - " : " (closed) - ") << max << (upperOpen ? " (open)" : " (closed)") <<  std::endl
			<< GWARNING;
		} else {
			throw gemfony_exception(
				g_error_streamer(DO_LOG, time_and_place)
					<< "In checkValueRange<int_type>(): Error!" << std::endl
					<< "Value " << val << " outside of allowed range " << std::endl
					<< min << (lowerOpen ? " (open) - " : " (closed) - ") << max << (upperOpen ? " (open)" : " (closed)") << std::endl
			);
		}
	}

	return val;
}

/******************************************************************************/
/**
 * Finds the minimum and maximum component in a vector of undefined types. This
 * function requires that x_type_undet can be compared using the usual operators.
 *
 * @param extDat The vector holding the data, for which extreme values should be calculated
 * @return A std::tuple holding the extreme values
 */
template<typename x_type_undet>
auto getMinMax(const std::vector<x_type_undet> &extDat) {
	// Do some error checking
	if (extDat.size() < (std::size_t) 2) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< "In GBasePlotter::getMinMax(1D): Error!" << std::endl
				<< "Got vector of invalid size " << extDat.size() << std::endl
		);
	}

	x_type_undet min = extDat.at(0), max = min;

	for (std::size_t i = 1; i < extDat.size(); i++) {
		if (extDat.at(i) < min) min = extDat.at(i);
		if (extDat.at(i) > max) max = extDat.at(i);
	}

	return std::tuple<x_type_undet, x_type_undet>{min, max};
}

/******************************************************************************/
/**
 * Find the minimum and maximum component in a vector of 2d-Tuples of undefined types.
 * This function requires that x_type_undet and y_type_undet can be compared using the
 * usual operators
 *
 * @param extDat The vector holding the data, for which extreme values should be calculated
 * @return A std::tuple holding the extreme values
 */
template<typename x_type_undet, typename y_type_undet>
auto getMinMax(const std::vector<std::tuple<x_type_undet, y_type_undet>> &extDat) {
	// Do some error checking
	if (extDat.size() < (std::size_t) 2) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< "In GBasePlotter::getMinMax(2D): Error!" << std::endl
				<< "Got vector of invalid size " << extDat.size() << std::endl
		);
	}

	x_type_undet minX = std::get<0>(extDat.at(0)), maxX = minX;
	y_type_undet minY = std::get<1>(extDat.at(0)), maxY = minY;

	for (std::size_t i = 1; i < extDat.size(); i++) {
		if (std::get<0>(extDat.at(i)) < minX) minX = std::get<0>(extDat.at(i));
		if (std::get<0>(extDat.at(i)) > maxX) maxX = std::get<0>(extDat.at(i));
		if (std::get<1>(extDat.at(i)) < minY) minY = std::get<1>(extDat.at(i));
		if (std::get<1>(extDat.at(i)) > maxY) maxY = std::get<1>(extDat.at(i));
	}

	return std::tuple<x_type_undet, x_type_undet, y_type_undet, y_type_undet>{minX, maxX, minY, maxY};
}

/******************************************************************************/
/**
 * Find the minimum and maximum component in a vector of 3d-Tuples of undefined types.
 * This function requires that x_type_undet, y_type_undet and z_type_undet can be compared
 * using the usual operators
 *
 * @param extDat The vector holding the data, for which extreme values should be calculated
 * @return A std::tuple holding the extreme values
 */
template<typename x_type_undet, typename y_type_undet, typename z_type_undet>
auto getMinMax(const std::vector<std::tuple<x_type_undet, y_type_undet, z_type_undet>> &extDat) {
	// Do some error checking
	if (extDat.size() < (std::size_t) 2) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< "In GBasePlotter::getMinMax(3D): Error!" << std::endl
				<< "Got vector of invalid size " << extDat.size() << std::endl
		);
	}

	x_type_undet minX = std::get<0>(extDat.at(0)), maxX = minX;
	y_type_undet minY = std::get<1>(extDat.at(0)), maxY = minY;
	z_type_undet minZ = std::get<2>(extDat.at(0)), maxZ = minZ;

	for (std::size_t i = 1; i < extDat.size(); i++) {
		if (std::get<0>(extDat.at(i)) < minX) minX = std::get<0>(extDat.at(i));
		if (std::get<0>(extDat.at(i)) > maxX) maxX = std::get<0>(extDat.at(i));
		if (std::get<1>(extDat.at(i)) < minY) minY = std::get<1>(extDat.at(i));
		if (std::get<1>(extDat.at(i)) > maxY) maxY = std::get<1>(extDat.at(i));
		if (std::get<2>(extDat.at(i)) < minZ) minZ = std::get<2>(extDat.at(i));
		if (std::get<2>(extDat.at(i)) > maxZ) maxZ = std::get<2>(extDat.at(i));
	}

	return std::tuple<x_type_undet, x_type_undet, y_type_undet, y_type_undet, z_type_undet, z_type_undet>{minX, maxX, minY, maxY, minZ, maxZ};
}

/******************************************************************************/
/**
 * Find the minimum and maximum component in a vector of 3d-Tuples of undefined types.
 * This function requires that x_type_undet, y_type_undet and z_type_undet can be compared
 * using the usual operators
 *
 * @param extDat The vector holding the data, for which extreme values should be calculated
 * @return A std::tuple holding the extreme values
 */
template<typename x_type_undet, typename y_type_undet, typename z_type_undet, typename w_type_undet>
auto getMinMax(const std::vector<std::tuple<x_type_undet, y_type_undet, z_type_undet, w_type_undet>> &extDat) {
	// Do some error checking
	if (extDat.size() < (std::size_t) 2) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< "In GBasePlotter::getMinMax(3D): Error!" << std::endl
				<< "Got vector of invalid size " << extDat.size() << std::endl
		);
	}

	x_type_undet minX = std::get<0>(extDat.at(0)), maxX = minX;
	y_type_undet minY = std::get<1>(extDat.at(0)), maxY = minY;
	z_type_undet minZ = std::get<2>(extDat.at(0)), maxZ = minZ;
	w_type_undet minW = std::get<3>(extDat.at(0)), maxW = minW;

	for (std::size_t i = 1; i < extDat.size(); i++) {
		if (std::get<0>(extDat.at(i)) < minX) minX = std::get<0>(extDat.at(i));
		if (std::get<0>(extDat.at(i)) > maxX) maxX = std::get<0>(extDat.at(i));
		if (std::get<1>(extDat.at(i)) < minY) minY = std::get<1>(extDat.at(i));
		if (std::get<1>(extDat.at(i)) > maxY) maxY = std::get<1>(extDat.at(i));
		if (std::get<2>(extDat.at(i)) < minZ) minZ = std::get<2>(extDat.at(i));
		if (std::get<2>(extDat.at(i)) > maxZ) maxZ = std::get<2>(extDat.at(i));
		if (std::get<3>(extDat.at(i)) < minW) minW = std::get<3>(extDat.at(i));
		if (std::get<3>(extDat.at(i)) > maxW) maxW = std::get<3>(extDat.at(i));
	}

	return std::tuple<x_type_undet, x_type_undet, y_type_undet, y_type_undet, z_type_undet, z_type_undet, w_type_undet, w_type_undet>{minX, maxX, minY, maxY, minZ, maxZ, minW, maxW};
}

/******************************************************************************/
/**
 * Calculates the mean value from a std::vector of floating point values
 *
 * @param parVec The vector of values for which the mean should be calculated
 * @return The mean value of parVec
 */
template<typename T>
T GMean(
	const std::vector<T> &parVec
	, typename std::enable_if<std::is_floating_point<T>::value>::type *dummy = nullptr
) {
	T mean = 0.;

#ifdef DEBUG
   if(parVec.empty()) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< "In T GMean(const std::vector<T>&): Error!" << std::endl
				<< "parVec has size 0" << std::endl
		);
   }
#endif /* DEBUG */

	typename std::vector<T>::const_iterator cit;
	for (cit = parVec.begin(); cit != parVec.end(); ++cit) {
		mean += *cit;
	}

	return mean / boost::numeric_cast<T>(parVec.size());
}

/******************************************************************************/
/**
 * Calculates the mean and standard deviation for a std::vector of floating point values
 *
 * @param parVec The vector of values for which the standard deviation should be calculated
 * @return A std::tuple holding the mean value and the standard deviation of the values stored in parVec
 */
template<typename T>
auto GStandardDeviation(
	const std::vector<T> &parVec, typename std::enable_if<std::is_floating_point<T>::value>::type *dummy = nullptr
) {
	T mean = GMean(parVec), sigma = 0.;

#ifdef DEBUG
   if(parVec.size() == 0) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< "In std::tuple<T,T> GStandardDeviation(const std::vector<T>&): Error!" << std::endl
				<< "parVec is empty" << std::endl
		);
   }
#endif /* DEBUG */

	// It is easy if the size is 1
	if (parVec.size() == 1) {
		return std::tuple<T, T>{parVec.at(0), 0.};
	}

	typename std::vector<T>::const_iterator cit;
	for (cit = parVec.begin(); cit != parVec.end(); ++cit) {
		sigma += GSQUARED(*cit - mean);

	}
	sigma /= parVec.size() - 1;
	sigma = sqrt(sigma);

	return std::tuple<T, T>{mean, sigma};
}

/******************************************************************************/
/**
 * Calculates the mean and standard deviation for each row of a "matrix" made up from several
 * std:vector<T> objects of equal size. E.g., if you have 5 std::vector<double> of size 10, you will
 * get back a std::vector<std::tuple<double, double>>, of size 10, holding the mean and standard
 * deviation of the corresponding positions in the 5 vectors.
 *
 * @param parVec The vectors for which the standard deviations should be calculated
 * @param result A std::vector holdung tuples with the mean and sigma values for each row
 */
template<typename T>
void GVecStandardDeviation(
	const std::vector<std::vector<T>> &parVec, std::vector<std::tuple<T, T>> &result,
	typename std::enable_if<std::is_floating_point<T>::value>::type *dummy = nullptr
) {

#ifdef DEBUG
   // Check that there are entries in the vector
   if(parVec.size() == 0) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< "In void GVecStandardDeviation(): Error!" << std::endl
				<< "parVec is empty" << std::endl
		);
   }

   // Check that the first entry has at least one component
   if(parVec.at(0).empty()) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< "In void GVecStandardDeviation(): Error!" << std::endl
				<< "parVec has empty component" << std::endl
		);
   }

   // Check that all entries have the same size
   if(parVec.size() > 1) {
      std::size_t sizeFirst = parVec.at(0).size(), pos=0;
      for(auto const &p: parVec) {
			if(p.size() != sizeFirst) {
				throw gemfony_exception(
					g_error_streamer(DO_LOG, time_and_place)
						<< "In void GVecStandardDeviation(): Error!" << std::endl
						<< "Found parVec component of different size: " << sizeFirst << " / " << pos << " / " << p.size() << std::endl
				);
         }
         pos++;
      }
   }

   // Now we know that there is at least one component with at least one entry,
   // and that all components have the same size. This is sufficient to calculate
   // the mean and standard deviation for each row of the "matrix" (made up from
   // the vectors,
#endif /* DEBUG */

	// Make sure our result vector is empty
	result.clear();

	typename std::vector<std::vector<T>>::const_iterator cit;
	for (std::size_t pos = 0; pos < parVec.at(0).size(); pos++) {
		std::vector<T> indPar;

		// Extract the individual parameters
		for (cit = parVec.begin(); cit != parVec.end(); ++cit) {
			indPar.push_back(cit->at(pos));
		}

		// Calculate the mean and standard deviation
		result.push_back(GStandardDeviation(indPar));
	}
}

/******************************************************************************/
/**
 * Calculation of pow for small positive integers using template metaprogramming
 */
template<std::size_t B, std::size_t E>
struct PowSmallPosInt {
	enum : std::size_t {
		result = B * PowSmallPosInt<B, E - 1>::result
	};
};

template<std::size_t B>
struct PowSmallPosInt<B, 2> {
	enum : std::size_t {
		result = B * B
	};
};

template<std::size_t B>
struct PowSmallPosInt<B, 1> {
	enum : std::size_t {
		result = B
	};
};

template<std::size_t B>
struct PowSmallPosInt<B, 0> {
	enum : std::size_t {
		result = 1
	};
};

/******************************************************************************/
/**
 * Takes two std::vector<> and subtracts each position of the second vector from the
 * corresponding position of the first vector. Note that we assume here that T understands
 * the operator-= . Note that after this function has been called, a will have changed.
 *
 * @param a The vector from whose elements numbers will be subtracted
 * @param b The vector whose elements will be subtracted from the elements of a
 */
template<typename T>
void subtractVec(
	std::vector<T> &a, const std::vector<T> &b
) {
#ifdef DEBUG
   // Do some error checking
   if(a.size() != b.size()) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< "In subtractVec(std::vector<T>, const std::vector<T>&): Error!" << std::endl
				<< "Found invalid sizes: " << a.size() << " / " << b.size() << std::endl
		);
   }
#endif /* DEBUG */

	typename std::vector<T>::iterator it_a;
	typename std::vector<T>::const_iterator cit_b;

	// Subtract the elements
	for (it_a = a.begin(), cit_b = b.begin(); it_a != a.end(); ++it_a, ++cit_b) {
		(*it_a) -= (*cit_b);
	}
}

/******************************************************************************/
/**
 * Takes two std::vector<> and adds each position of the second vector to the
 * corresponding position of the first vector. Note that we assume here that T understands
 * the operator+= . Note that after this function has been called, a will have changed.
 *
 * @param a The vector to whose elements numbers will be added
 * @param b The vector whose elements will be added to the elements of a
 */
template<typename T>
void addVec(
	std::vector<T> &a, const std::vector<T> &b
) {
#ifdef DEBUG
   // Do some error checking
   if(a.size() != b.size()) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< "In addVec(std::vector<T>, const std::vector<T>&): Error!" << std::endl
				<< "Found invalid sizes: " << a.size() << " / " << b.size() << std::endl
		);
   }
#endif /* DEBUG */

	typename std::vector<T>::iterator it_a;
	typename std::vector<T>::const_iterator cit_b;

	// Subtract the elements
	for (it_a = a.begin(), cit_b = b.begin(); it_a != a.end(); ++it_a, ++cit_b) {
		(*it_a) += (*cit_b);
	}
}

/******************************************************************************/
/**
 * Multiplies each position of a std::vector<> with a constant. Note that we assume here that
 * T understands the operator*= . Note that after this function has been called, a will have changed.
 *
 * @param a The vector to whose elements numbers will be added
 * @param c The constant which will be multiplied with each position of a
 */
template<typename T>
void multVecConst(
	std::vector<T> &a, const T &c
) {
	typename std::vector<T>::iterator it_a;

	// Subtract the elements
	for (it_a = a.begin(); it_a != a.end(); ++it_a) {
		(*it_a) *= c;
	}
}

/******************************************************************************/
/**
 * Assigns a constant value to each position of the vector. Note that we assume here that
 * T understands the operator*= . Note that after this function has been called, a will have changed.
 *
 * @param a The vector to whose elements c will be assigned
 * @param c The constant which will be assigned each position of a
 */
template<typename T>
void assignVecConst(
	std::vector<T> &a, const T &c
) {
	typename std::vector<T>::iterator it_a;

	// Assign c to each element
	for (it_a = a.begin(); it_a != a.end(); ++it_a) {
		(*it_a) = c;
	}
}

/******************************************************************************/
/**
 * Summs up the x- and y-components individually of a vector of 2d-tuples
 */
template<typename fp_type>
std::tuple<fp_type, fp_type> sumTupleVec(
	const std::vector<std::tuple<fp_type, fp_type>> &dataPoints,
	typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
	std::tuple<fp_type, fp_type> result = std::tuple<fp_type, fp_type>(fp_type(0.), fp_type(0.));;

	typename std::vector<std::tuple<fp_type, fp_type>>::const_iterator cit;
	for (cit = dataPoints.begin(); cit != dataPoints.end(); ++cit) {
		std::get<0>(result) += std::get<0>(*cit);
		std::get<1>(result) += std::get<1>(*cit);
	}

	return result;
}

/******************************************************************************/
/**
 * Summs up the squares of x- and y-components individually of a vector of 2d-tuples
 */
template<typename fp_type>
std::tuple<fp_type, fp_type> squareSumTupleVec(
	const std::vector<std::tuple<fp_type, fp_type>> &dataPoints,
	typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
	std::tuple<fp_type, fp_type> result = std::tuple<fp_type, fp_type>(fp_type(0.), fp_type(0.));

	typename std::vector<std::tuple<fp_type, fp_type>>::const_iterator cit;
	for (cit = dataPoints.begin(); cit != dataPoints.end(); ++cit) {
		std::get<0>(result) += gpow(std::get<0>(*cit), 2.);
		std::get<1>(result) += gpow(std::get<1>(*cit), 2.);
	}

	return result;
}

/******************************************************************************/
/**
 * Summs up the product of x- and y-components of a vector of 2d-tuples
 */
template<typename fp_type>
fp_type productSumTupleVec(
	const std::vector<std::tuple<fp_type, fp_type>> &dataPoints,
	typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
	fp_type result = fp_type(0.);

	typename std::vector<std::tuple<fp_type, fp_type>>::const_iterator cit;
	for (cit = dataPoints.begin(); cit != dataPoints.end(); ++cit) {
		result += (std::get<0>(*cit) * std::get<1>(*cit));
	}

	return result;
}

/******************************************************************************/
/**
 * Calculates the "square deviation" of a set of floating point tuples from
 * a line defined through a + b*x .
 *
 * @param dataPoints A vector of bi-tuples with x-y data points
 * @param a The offset of a line
 * @param b The slope of a line
 * @return The square deviation of the data points from the line
 */
template<typename fp_type>
fp_type squareDeviation(
	const std::vector<std::tuple<fp_type, fp_type>> &dataPoints, const fp_type &a, const fp_type &b,
	typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
	fp_type result = fp_type(0);
	typename std::vector<std::tuple<fp_type, fp_type>>::const_iterator cit;
	for (cit = dataPoints.begin(); cit != dataPoints.end(); ++cit) {
		result += gpow(std::get<1>(*cit) - a - b * std::get<0>(*cit), 2.);
	}
	return result;
}

/******************************************************************************/
/**
 * Calculates the parameters a and b of a regression line, plus errors. The return
 * value is a std::tuple of four fp_type values: a, error_a, b, error_b, with the
 * line being defined by L(x)=a+b*x .
 *
 * @param dataPoints A vector of data points to which the lines parameters should fit
 * @return Regression parameters for a line defined by the input data points
 */
template<typename fp_type>
auto getRegressionParameters(
	const std::vector<std::tuple<fp_type, fp_type>> &dataPoints,
	typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
	if (dataPoints.empty()) {
		return std::tuple<fp_type, fp_type, fp_type, fp_type>{fp_type(0.), fp_type(0.), fp_type(0.), fp_type(0.)};
	}

	fp_type a = fp_type(0), b = fp_type(0);
	fp_type n = fp_type(dataPoints.size());

	std::tuple<fp_type, fp_type> sum_xy = sumTupleVec(dataPoints);
	fp_type sum_x = std::get<0>(sum_xy);
	fp_type sum_y = std::get<1>(sum_xy);

	std::tuple<fp_type, fp_type> sq_sum_xy = squareSumTupleVec(dataPoints);
	fp_type sq_sum_x = std::get<0>(sq_sum_xy);
	fp_type sq_sum_y = std::get<1>(sq_sum_xy);

	fp_type prod_sum_xy = productSumTupleVec(dataPoints);

	a = (sum_y * sq_sum_x - sum_x * prod_sum_xy) / (n * sq_sum_x - gpow(sum_x, 2.));
	b = (n * prod_sum_xy - sum_x * sum_y) / (n * sq_sum_x - gpow(sum_x, 2.));

	fp_type dev = squareDeviation(dataPoints, a, b);

	fp_type sigma_a = gsqrt(dev / (n - 2.)) * gsqrt(sq_sum_x / (n * sq_sum_x - gpow(sum_x, 2.)));
	fp_type sigma_b = gsqrt(dev / (n - 2.)) * gsqrt(n / (n * sq_sum_x - gpow(sum_x, 2.)));

	return std::tuple<fp_type, fp_type, fp_type, fp_type>{a, sigma_a, b, sigma_b};
}

/******************************************************************************/
/**
 * Calculates the error of a function f=s/p , where s and p are independent
 * quantities, each with its own error. Returns:
 * - The sleep time
 * - The error on the sleep-time (always 0)
 * - the quantity s/p
 * - error on s/p
 * s and p have the same structure
 */
template<typename fp_type>
auto getRatioError(
	const std::tuple<fp_type, fp_type, fp_type, fp_type> &s, const std::tuple<fp_type, fp_type, fp_type, fp_type> &p,
	typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
	// p may not ne 0
	if (0. == std::get<2>(p)) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< "In getRatioError(): Error!" << std::endl
				<< "Attempted division by 0." << std::endl
		);
	}

	fp_type sleep_time = std::get<0>(s);

	// Check that the sleep-times for s and p are the same
	if (sleep_time != std::get<0>(p)) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< "In getRatioError(): Error!" << std::endl
				<< "Sleep times differ: " << sleep_time << " / " << std::get<0>(p) << std::endl
		);
	}

	fp_type s_val = std::get<2>(s);
	fp_type s_err = std::get<3>(s);
	fp_type p_val = std::get<2>(p);
	fp_type p_err = std::get<3>(p);

	return std::tuple<fp_type, fp_type, fp_type, fp_type>{
		sleep_time
		, 0.
		, s_val / p_val
		, gsqrt(gpow(s_err / p_val, fp_type(2.)) + gpow(s_val * p_err / gpow(p_val, fp_type(2.)), fp_type(2.)))
	};
}

/******************************************************************************/
/**
 * Calculates the error for a function f=s/p , where s and p are independent
 * quantities, each with its own error. The function is applied to a std::vector
 * of different s and p (together with their errors). It returns a vector
 * of s/p together with their errors.
 */
template<typename fp_type>
std::vector<std::tuple<fp_type, fp_type, fp_type, fp_type>> getRatioErrors(
	const std::vector<std::tuple<fp_type, fp_type, fp_type, fp_type>> &sn,
	const std::vector<std::tuple<fp_type, fp_type, fp_type, fp_type>> &pn,
	typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
	// Check that both vectors have the same size, otherwise complain
	if (sn.size() != pn.size()) {
		throw gemfony_exception(
			g_error_streamer(DO_LOG, time_and_place)
				<< "In getRatioErrors(): Error!" << std::endl
				<< "Vectors have invalid sizes: " << sn.size() << " / " << pn.size() << std::endl
		);
	}

	std::vector<std::tuple<fp_type, fp_type, fp_type, fp_type>> spn;
	typename std::vector<std::tuple<fp_type, fp_type, fp_type, fp_type>>::const_iterator s_cit, p_cit;
	for (s_cit = sn.begin(), p_cit = pn.begin(); s_cit != sn.end(); ++s_cit, ++p_cit) {
		spn.push_back(getRatioError(*s_cit, *p_cit));
	}

	return spn;
}

/******************************************************************************/
/**
 * This function checks whether a given floating point value is "close" to a given
 * target value, with a maximum difference provided as a parameter
 */
template<typename fp_type>
bool isClose(
	fp_type val
	, fp_type target = 0
	, fp_type margin = fp_type(0.00001)
	, typename std::enable_if<std::is_floating_point<fp_type>::value>::type *dummy = nullptr
) {
	return (gfabs(val - target) <= margin);
}

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

} /* namespace Common */
} /* namespace Gem */


//
// Copyright (c) 2016 - 2017 Mesh Consultants Inc.
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.
//

#include "Mesh_SmoothMesh.h"

#include <assert.h>

#include <iostream>
#include <vector>

#include <Eigen/Core>

#pragma warning(push, 0)
#include <igl/adjacency_list.h>
#pragma warning(pop)

#include "ConversionUtilities.h"
#include "TriMesh.h"

using namespace Urho3D;

String Mesh_SmoothMesh::iconTexture = "Textures/Icons/Mesh_SmoothMesh.png";

Mesh_SmoothMesh::Mesh_SmoothMesh(Context* context) : IoComponentBase(context, 3, 1)
{
	SetName("SmoothMesh");
	SetFullName("Smooth Mesh");
	SetDescription("Perform smoothing on triangle mesh");
	SetGroup(IoComponentGroup::MESH);
	SetSubgroup("Operators");

	inputSlots_[0]->SetName("Mesh");
	inputSlots_[0]->SetVariableName("M");
	inputSlots_[0]->SetDescription("Mesh before smoothing");
	inputSlots_[0]->SetVariantType(VariantType::VAR_VARIANTMAP);
	inputSlots_[0]->SetDataAccess(DataAccess::ITEM);

	inputSlots_[1]->SetName("Iterations");
	inputSlots_[1]->SetVariableName("I");
	inputSlots_[1]->SetDescription("Number of smoothing iterations");
	inputSlots_[1]->SetVariantType(VariantType::VAR_INT);
	inputSlots_[1]->SetDataAccess(DataAccess::ITEM);
	inputSlots_[1]->SetDefaultValue(Variant(1));
	inputSlots_[1]->DefaultSet();

	inputSlots_[2]->SetName("Target Factor");
	inputSlots_[2]->SetVariableName("F");
	inputSlots_[2]->SetDescription("Move towards smoothed target by factor");
	inputSlots_[2]->SetVariantType(VariantType::VAR_FLOAT);
	inputSlots_[2]->SetDataAccess(DataAccess::ITEM);
	inputSlots_[2]->SetDefaultValue(Variant(0.5f));
	inputSlots_[2]->DefaultSet();

	outputSlots_[0]->SetName("Mesh");
	outputSlots_[0]->SetVariableName("M");
	outputSlots_[0]->SetDescription("Mesh after smoothing");
	outputSlots_[0]->SetVariantType(VariantType::VAR_VARIANTMAP);
	outputSlots_[0]->SetDataAccess(DataAccess::ITEM);
}

void Mesh_SmoothMesh::SolveInstance(
	const Vector<Variant>& inSolveInstance,
	Vector<Variant>& outSolveInstance
)
{
	assert(inSolveInstance.Size() == inputSlots_.Size());
	assert(outSolveInstance.Size() == outputSlots_.Size());

	///////////////////
	// VERIFY & EXTRACT

	// Verify input slot 0
	Variant inMesh = inSolveInstance[0];
	if (!TriMesh_Verify(inMesh)) {
		URHO3D_LOGWARNING("M must be a valid mesh.");
		outSolveInstance[0] = Variant();
		return;
	}
	// Verify input slot 1
	VariantType type1 = inSolveInstance[1].GetType();
	if (type1 != VariantType::VAR_INT) {
		URHO3D_LOGWARNING("I must be a valid integer.");
		outSolveInstance[0] = Variant();
		return;
	}
	int steps = inSolveInstance[1].GetInt();
	if (steps < 0) {
		steps = 0;
	}
	// Verify input slot 2
	VariantType type2 = inSolveInstance[2].GetType();
	if (type2 != VariantType::VAR_FLOAT) {
		URHO3D_LOGWARNING("F must be a valid float.");
		outSolveInstance[0] = Variant();
		return;
	}
	float targetFactor = inSolveInstance[2].GetFloat();
	if (targetFactor < 0.0f || targetFactor > 1.0f) {
		URHO3D_LOGWARNING("F must be in range 0 <= F <= 1.");
		outSolveInstance[0] = Variant();
		return;
	}

	///////////////////
	// COMPONENT'S WORK

	Eigen::MatrixXf V;
	Eigen::MatrixXi F;
	bool loadSuccess = IglMeshToMatrices(inMesh, V, F);
	Variant outMesh;

	if (loadSuccess) {
		Eigen::MatrixXf W = V;

		std::vector<std::vector<unsigned> > adj;
		igl::adjacency_list(F, adj);

		for (int index = 0; index < steps; ++index) {
			for (unsigned i = 0; i < adj.size(); ++i) {
				std::vector<unsigned> cur_adj = adj[i];
				Eigen::RowVector3f v(0.0f, 0.0f, 0.0f);
				for (unsigned j = 0; j < cur_adj.size(); ++j) {
					v += W.row(cur_adj[j]);
				}
				if (cur_adj.size() > 0) {
					float scalar = 1.0f / cur_adj.size();
					v = scalar * v;
				}
				W.row(i) = v;
			}
		}

		for (unsigned i = 0; i < V.rows(); ++i) {
			V.row(i) = (1.0f - targetFactor) * V.row(i) + targetFactor * W.row(i);
		}
		//outMesh = IglMatricesToMesh(V, F);
		outMesh = TriMesh_Make(V, F);
	}
	else {
		URHO3D_LOGWARNING("Smooth operation failed.");
		outSolveInstance[0] = Variant();
		return;
	}

	/////////////////
	// ASSIGN OUTPUTS

	outSolveInstance[0] = outMesh;
}


//
//  Copyright © 2015 Claus Christmann <hcc |ä| gatech.edu>.
//
//  Licensed under the Apache License, Version 2.0 (the "License");
//  you may not use this file except in compliance with the License.
//  You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
//

#ifndef __CREATE_DUAL_2D_GRAPH_HPP__
#define __CREATE_DUAL_2D_GRAPH_HPP__

#include <vector>

#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/planar_face_traversal.hpp>

#include "Geometry/geometry.h"

namespace Graph {

template< typename VertexPositionMap,
          typename Ring  = Geometry::Polygon_2D::ring_type
        >
class FaceGeometry_visitor : public boost::planar_face_traversal_visitor
{
public:

  using Point = boost::range_value<Ring>::type;

  FaceGeometry_visitor(VertexPositionMap vertexPositionMap)
  :pos_map(vertexPositionMap)
  {
    using namespace boost;
    BOOST_CONCEPT_ASSERT(( geometry::concept::Ring< typename Ring > ));
    BOOST_CONCEPT_ASSERT(( geometry::concept::Point< typename Point > ));

//     BOOST_CONCEPT_ASSERT(( ));
    BOOST_CONCEPT_ASSERT(( geometry::concept::Point< typename property_traits<PositionMap>::value_type >  ));
  };

  void begin_face()
  {
    // start by creating a new face geometry
    face_geometry.clear();
  };

  void end_face()
  {
    boost::geometry::correct(face_geometry);
    geometries.push_back(face_geometry);

//       std::cout
//         << boost::geometry::dsv(face_geometry)
//         << " -> "
//         << boost::geometry::area(face_geometry) << std::endl;
  };

  std::vector<FaceGeometry> getFaceGeometries() const
  { return geometries; };

protected:
  Ring face_geometry;
  VertexPositionMap pos_map;
  std::vector<Ring> geometries;

};

/** \brief A planar graph visitor to extract the 2D positioned dual graph.
  *
  * \note The visitor logic to create the dual graph is copy-and-pasted from
  * http://aawblog.wordpress.com/2009/12/16/boost-graph-library-planar-dual/
  * (Retrieved 8/16/2013)
  *
  */
template< typename InputGraph,
          typename OutputGraph,
          typename EdgeIndexMap,
          typename VertexPositionMap>
class dual_graph_2D_visitor : public FaceGeometry_visitor<VertexPositionMap>
{
public:

  using vertex_t  = typename boost::graph_traits<OutputGraph>::vertex_descriptor;
  using edge_t    = typename boost::graph_traits<InputGraph>::edge_descriptor;
  using vertex_vector_t = typename std::vector<vertex_t>;
  using edge_to_face_map_t = boost::iterator_property_map< typename vertex_vector_t::iterator, EdgeIndexMap >;

  dual_graph_2D_visitor(InputGraph& graph_in,
                        OutputGraph& graphDual_out,
                        EdgeIndexMap edgeIndexMap,
                        VertexPositionMap vertexPosMap
                        )
    :FaceGeometry_visitor<VertexPositionMap>(vertexPosMap)
    ,g(graph_in)
    ,dual_g(graphDual_out)
    ,em(edgeIndexMap)
    ,edge_to_face_vector(boost::num_edges(g), boost::graph_traits<OutputGraph>::null_vertex())
    ,edge_to_face( edge_to_face_vector.begin(), em)
  {};
  ~dual_graph_2D_visitor() = default;

  void begin_face()
  {
    FaceGeometry_visitor<VertexPositionMap>::begin_face();
    current_face = boost::add_vertex(dual_g);
  };

  template< typename Edge >
  void next_edge(Edge e)
  {
    vertex_t existing_face = edge_to_face[e];
    if (existing_face == boost::graph_traits<OutputGraph>::null_vertex())
    {
      edge_to_face[e] = current_face;
    }
    else
    {
      boost::add_edge(existing_face, current_face, dual_g);
    }
  };

  void end_face()
  {
    FaceGeometry_visitor<VertexPositionMap>::end_face();
    Geometry::Point_2D face_centroid;
    boost::geometry::centroid(FaceGeometry_visitor<VertexPositionMap>::face_geometry,face_centroid);

    boost::put(boost::vertex_position_2D,dual_g,current_face,face_centroid);
  };

protected:

  InputGraph& g; ///< \brief The Voronoi graph.
  OutputGraph& dual_g; ///< \brief  The obstacle graph.
  EdgeIndexMap em;
  vertex_t current_face; ///< \brief The currently processed face of the Voronoi graph.
  vertex_vector_t edge_to_face_vector;
  edge_to_face_map_t edge_to_face;
};

template <typename InputGraph,
          typename OutputGraph,
          typename PlanarEmbedding,
          typename EdgeIndexMap,
          typename VertexPositionMap
          >
void create_dual_2D_graph(InputGraph& g,
                          OutputGraph& dual_g,
                          PlanarEmbedding embedding,
                          EdgeIndexMap em,
                          VertexPositionMap pm)
{
  using namespace boost;

  dual_graph_2D_visitor<InputGraph, OutputGraph, EdgeIndexMap, VertexPositionMap>
  visitor(g, dual_g, em, pm);
  planar_face_traversal(g, embedding, visitor, em);
}

template <typename InputGraph,
          typename OutputGraph,
          typename PlanarEmbedding
          >
void create_dual_2D_graph(InputGraph& g,
                          OutputGraph& dual_g,
                          PlanarEmbedding embedding
                          )
{
  using namespace boost;
  create_dual_2D_graph(g, dual_g, embedding,
                        get(edge_index,g),
                        get(vertex_position_2D,g)
                      );
}

} // namespace Graph

#endif //__CREATE_DUAL_2D_GRAPH_HPP__


/* ============================================================================
 * Copyright (c) 2009-2016 BlueQuartz Software, LLC
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright notice, this
 * list of conditions and the following disclaimer in the documentation and/or
 * other materials provided with the distribution.
 *
 * Neither the name of BlueQuartz Software, the US Air Force, nor the names of its
 * contributors may be used to endorse or promote products derived from this software
 * without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE
 * USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * The code contained herein was partially funded by the followig contracts:
 *    United States Air Force Prime Contract FA8650-07-D-5800
 *    United States Air Force Prime Contract FA8650-10-D-5210
 *    United States Prime Contract Navy N00173-07-C-2068
 *
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

/* ============================================================================
 * DerivativeHelpers re-implements the exterior derivative calculations
 * from the following vtk modules:
 *
 * * vtkLine.cxx
 *   - re-implemented vtkLine::Derivatives to EdgeDeriv::operator()
 * * vtkTriangle.cxx
 *   - re-implemented vtkTriangle::Derivatives to TriangleDeriv::operator()
 * * vtkQuad.cxx
 *   - re-implemented vtkQuad::Derivatives to QuadDeriv::operator()
 * * vtkTetra.cxx
 *   - re-implemented vtkTetra::Derivatives to TetDeriv::operator()
 * * vtkHexahedron.cxx
 *   - re-implemented vtkHexahedron::Derivatives to HexDeriv::operator()
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ */

#include "DerivativeHelpers.h"

#include <Eigen/Eigenvalues>
#include <Eigen/LU>

#include "SIMPLib/Geometry/EdgeGeom.h"
#include "SIMPLib/Geometry/HexahedralGeom.h"
#include "SIMPLib/Geometry/QuadGeom.h"
#include "SIMPLib/Geometry/TetrahedralGeom.h"
#include "SIMPLib/Geometry/TriangleGeom.h"
#include "SIMPLib/Math/GeometryMath.h"
#include "SIMPLib/Math/MatrixMath.h"

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void DerivativeHelpers::EdgeDeriv::operator()(EdgeGeom* edges, size_t edgeId, double values[2], double derivs[3])
{
  float vert0_f[3] = {0.0f, 0.0f, 0.0f};
  float vert1_f[3] = {0.0f, 0.0f, 0.0f};
  double vert0[3] = {0.0, 0.0, 0.0};
  double vert1[3] = {0.0, 0.0, 0.0};
  double delta[3] = {0.0, 0.0, 0.0};
  size_t verts[2] = {0, 0};

  edges->getVertsAtEdge(edgeId, verts);
  edges->getCoords(verts[0], vert0_f);
  edges->getCoords(verts[1], vert1_f);

  std::copy(vert0_f, vert0_f + 3, vert0);
  std::copy(vert1_f, vert1_f + 3, vert1);

  for(size_t i = 0; i < 3; i++)
  {
    delta[i] = vert1[i] - vert0[i];
  }

  for(size_t i = 0; i < 3; i++)
  {
    if(delta[i] != 0.0)
    {
      derivs[i] = (values[1] - values[0]) / delta[i];
    }
    else
    {
      derivs[i] = 0.0;
    }
  }
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void DerivativeHelpers::TriangleDeriv::operator()(TriangleGeom* triangles, size_t triId, double values[3], double derivs[3])
{
  float vert0_f[3] = {0.0f, 0.0f, 0.0f};
  float vert1_f[3] = {0.0f, 0.0f, 0.0f};
  float vert2_f[3] = {0.0f, 0.0f, 0.0f};
  double vert0[3] = {0.0, 0.0, 0.0};
  double vert1[3] = {0.0, 0.0, 0.0};
  double vert2[3] = {0.0, 0.0, 0.0};
  double vert0_2d[2] = {0.0, 0.0};
  double vert1_2d[2] = {0.0, 0.0};
  double vert2_2d[2] = {0.0, 0.0};
  double vector20[3] = {0.0, 0.0, 0.0};
  double basis1[3] = {0.0, 0.0, 0.0};
  double basis2[3] = {0.0, 0.0, 0.0};
  double mag_basis1 = 0.0;
  double mag_basis2 = 0.0;
  double normal[3] = {0.0, 0.0, 0.0};
  double shapeFunctions[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double sum[2] = {0.0, 0.0};
  double dBydx = 0.0;
  double dBydy = 0.0;
  size_t verts[3] = {0, 0, 0};

  triangles->getVertsAtTri(triId, verts);
  triangles->getCoords(verts[0], vert0_f);
  triangles->getCoords(verts[1], vert1_f);
  triangles->getCoords(verts[2], vert2_f);

  std::copy(vert0_f, vert0_f + 3, vert0);
  std::copy(vert1_f, vert1_f + 3, vert1);
  std::copy(vert2_f, vert2_f + 3, vert2);

  GeometryMath::FindPlaneNormalVector(vert0, vert1, vert2, normal);
  MatrixMath::Normalize3x1(normal);

  for(size_t i = 0; i < 3; i++)
  {
    basis1[i] = vert1[i] - vert0[i];
    vector20[i] = vert2[i] - vert0[i];
  }

  MatrixMath::CrossProduct(normal, basis1, basis2);

  mag_basis1 = MatrixMath::Magnitude3x1(basis1);
  mag_basis2 = MatrixMath::Magnitude3x1(basis2);

  if(mag_basis1 <= 0.0 || mag_basis2 <= 0.0)
  {
    for(size_t i = 0; i < 3; i++)
    {
      derivs[i] = 0.0;
    }
    return;
  }

  MatrixMath::Normalize3x1(basis1);
  MatrixMath::Normalize3x1(basis2);

  // Project the 3D triangle onto a 2D local system
  // Basis vectors for 2D system are the vector between triangle vertex 1 and 0 (basis1)
  // and the normal of the plane defined by the triangle normal and basis1 (basis2)
  vert0_2d[0] = vert0_2d[1] = 0.0;
  vert1_2d[0] = mag_basis1;
  vert1_2d[1] = 0.0;
  vert2_2d[0] = MatrixMath::DotProduct3x1(vector20, basis1);
  vert2_2d[1] = MatrixMath::DotProduct3x1(vector20, basis2);

  // Compute interpolation function derivatives
  triangles->getShapeFunctions(nullptr, shapeFunctions);

  // Compute Jacobian and inverse Jacobian using Eigen
  // Jacobian is constant for a triangle, so inverse must exist
  double jPtr[4] = {0.0, 0.0, 0.0, 0.0};

  jPtr[0] = vert1_2d[0] - vert0_2d[0];
  jPtr[1] = vert1_2d[1] - vert0_2d[1];
  jPtr[2] = vert2_2d[0] - vert0_2d[0];
  jPtr[3] = vert2_2d[1] - vert0_2d[1];

  Eigen::Map<TriangleJacobian> jMat(jPtr);
  TriangleJacobian jMatI;

  jMatI = jMat.inverse();

  // Loop over derivative values. For each set of values, compute
  // derivatives in local 2D system and then transform into original 3D system
  for(size_t i = 0; i < 3; i++)
  {
    sum[0] += shapeFunctions[i] * values[i];
    sum[1] += shapeFunctions[3 + i] * values[i];
  }

  dBydx = sum[0] * jMatI(0, 0) + sum[1] * jMatI(0, 1);
  dBydy = sum[0] * jMatI(1, 0) + sum[1] * jMatI(1, 1);

  derivs[0] = dBydx * basis1[0] + dBydy * basis2[0];
  derivs[1] = dBydx * basis1[1] + dBydy * basis2[1];
  derivs[2] = dBydx * basis1[2] + dBydy * basis2[2];
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void DerivativeHelpers::QuadDeriv::operator()(QuadGeom* quads, size_t quadId, double values[4], double derivs[3])
{
  float vert0_f[3] = {0.0f, 0.0f, 0.0f};
  float vert1_f[3] = {0.0f, 0.0f, 0.0f};
  float vert2_f[3] = {0.0f, 0.0f, 0.0f};
  float vert3_f[3] = {0.0f, 0.0f, 0.0f};
  double vert0[3] = {0.0, 0.0, 0.0};
  double vert1[3] = {0.0, 0.0, 0.0};
  double vert2[3] = {0.0, 0.0, 0.0};
  double vert3[3] = {0.0, 0.0, 0.0};
  double vert0_2d[2] = {0.0, 0.0};
  double vert1_2d[2] = {0.0, 0.0};
  double vert2_2d[2] = {0.0, 0.0};
  double vert3_2d[2] = {0.0, 0.0};
  double vector20[3] = {0.0, 0.0, 0.0};
  double vector30[3] = {0.0, 0.0, 0.0};
  double basis1[3] = {0.0, 0.0, 0.0};
  double basis2[3] = {0.0, 0.0, 0.0};
  double mag_basis1 = 0.0;
  double mag_basis2 = 0.0;
  double normal[3] = {0.0, 0.0, 0.0};
  double shapeFunctions[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  double pCoords[3]{0.0, 0.0, 0.0};
  double sum[2] = {0.0, 0.0};
  double dBydx = 0.0;
  double dBydy = 0.0;
  size_t verts[4] = {0, 0, 0, 0};

  quads->getVertsAtQuad(quadId, verts);
  quads->getCoords(verts[0], vert0_f);
  quads->getCoords(verts[1], vert1_f);
  quads->getCoords(verts[2], vert2_f);
  quads->getCoords(verts[3], vert3_f);

  std::copy(vert0_f, vert0_f + 3, vert0);
  std::copy(vert1_f, vert1_f + 3, vert1);
  std::copy(vert2_f, vert2_f + 3, vert2);
  std::copy(vert3_f, vert3_f + 3, vert3);

  GeometryMath::FindPlaneNormalVector(vert0, vert1, vert2, normal);
  MatrixMath::Normalize3x1(normal);

  // If vertices 0, 1, & 2 are co-linear, use vertex 3 to find the normal
  if(normal[0] == 0.0 && normal[1] == 0.0 && normal[2] == 0.0)
  {
    GeometryMath::FindPlaneNormalVector(vert0, vert1, vert2, normal);
    MatrixMath::Normalize3x1(normal);
  }

  for(size_t i = 0; i < 3; i++)
  {
    basis1[i] = vert1[i] - vert0[i];
    vector20[i] = vert2[i] - vert0[i];
    vector30[i] = vert3[i] - vert0[i];
  }

  MatrixMath::CrossProduct(normal, basis1, basis2);

  mag_basis1 = MatrixMath::Magnitude3x1(basis1);
  mag_basis2 = MatrixMath::Magnitude3x1(basis2);

  if(mag_basis1 <= 0.0 || mag_basis2 <= 0.0)
  {
    for(size_t i = 0; i < 3; i++)
    {
      derivs[i] = 0.0;
    }
    return;
  }

  MatrixMath::Normalize3x1(basis1);
  MatrixMath::Normalize3x1(basis2);

  // Project the 3D quad onto a 2D local system
  // Basis vectors for 2D system are the vector between quad vertex 1 and 0 (basis1)
  // and the normal of the plane defined by the quad normal and basis1 (basis2)
  vert0_2d[0] = vert0_2d[1] = 0.0;
  vert1_2d[0] = mag_basis1;
  vert1_2d[1] = 0.0;
  vert2_2d[0] = MatrixMath::DotProduct3x1(vector20, basis1);
  vert2_2d[1] = MatrixMath::DotProduct3x1(vector20, basis2);
  vert3_2d[0] = MatrixMath::DotProduct3x1(vector30, basis1);
  vert3_2d[1] = MatrixMath::DotProduct3x1(vector30, basis2);

  quads->getParametricCenter(pCoords);
  quads->getShapeFunctions(pCoords, shapeFunctions);

  // Compute Jacobian and inverse Jacobian using Eigen
  double jPtr[4] = {0.0, 0.0, 0.0, 0.0};

  jPtr[0] = vert0_2d[0] * shapeFunctions[0] + vert1_2d[0] * shapeFunctions[1] + vert2_2d[0] * shapeFunctions[2] + vert3_2d[0] * shapeFunctions[3];
  jPtr[1] = vert0_2d[1] * shapeFunctions[0] + vert1_2d[1] * shapeFunctions[1] + vert2_2d[1] * shapeFunctions[2] + vert3_2d[1] * shapeFunctions[3];
  jPtr[2] = vert0_2d[0] * shapeFunctions[4] + vert1_2d[0] * shapeFunctions[5] + vert2_2d[0] * shapeFunctions[6] + vert3_2d[0] * shapeFunctions[7];
  jPtr[3] = vert0_2d[1] * shapeFunctions[4] + vert1_2d[1] * shapeFunctions[5] + vert2_2d[1] * shapeFunctions[6] + vert3_2d[1] * shapeFunctions[7];

  Eigen::Map<QuadJacobian> jMat(jPtr);
  QuadJacobian jMatI;
  bool invertible = false;

  jMat.computeInverseWithCheck(jMatI, invertible);

  // Jacobian is non-constant for a quad, so must check if inverse exists
  // If the Jacobian is not invertible, set derivatives to 0
  if(!invertible)
  {
    for(size_t i = 0; i < 3; i++)
    {
      derivs[i] = 0.0;
    }
  }

  // Loop over derivative values. For each set of values, compute
  // derivatives in local 2D system and then transform into original 3D system
  for(size_t i = 0; i < 4; i++)
  {
    sum[0] += shapeFunctions[i] * values[i];
    sum[1] += shapeFunctions[4 + i] * values[i];
  }

  dBydx = sum[0] * jMatI(0, 0) + sum[1] * jMatI(0, 1);
  dBydy = sum[0] * jMatI(1, 0) + sum[1] * jMatI(1, 1);

  derivs[0] = dBydx * basis1[0] + dBydy * basis2[0];
  derivs[1] = dBydx * basis1[1] + dBydy * basis2[1];
  derivs[2] = dBydx * basis1[2] + dBydy * basis2[2];
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void DerivativeHelpers::TetDeriv::operator()(TetrahedralGeom* tets, size_t tetId, double values[4], double derivs[3])
{
  double shapeFunctions[12] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  size_t verts[4] = {0, 0, 0, 0};
  double sum[3] = {0.0, 0.0, 0.0};

  tets->getShapeFunctions(nullptr, shapeFunctions);

  // Compute 3x3 Jacobian from vertex coordinates and tet shape functions,
  // then find the inverse Jacobian using Eigen
  double jPtr[9] = {
      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
  };

  tets->getVertsAtTet(tetId, verts);

  for(size_t i = 0; i < 4; i++)
  {
    float tmpCoords_f[3] = {0.0, 0.0, 0.0};
    double tmpCoords[3] = {0.0, 0.0, 0.0};
    tets->getCoords(verts[i], tmpCoords_f);
    std::copy(tmpCoords_f, tmpCoords_f + 3, tmpCoords);
    for(size_t j = 0; j < 3; j++)
    {
      jPtr[j] += tmpCoords[j] * shapeFunctions[i];
      jPtr[j + 3] += tmpCoords[j] * shapeFunctions[4 + i];
      jPtr[j + 6] += tmpCoords[j] * shapeFunctions[8 + i];
    }
  }

  Eigen::Map<TetJacobian> jMat(jPtr);
  TetJacobian jMatI;
  bool invertible = false;

  jMat.computeInverseWithCheck(jMatI, invertible);

  // Jacobian is non-constant for a tet, so must check if inverse exists
  // If the Jacobian is not invertible, set derivatives to 0
  if(!invertible)
  {
    for(size_t i = 0; i < 3; i++)
    {
      derivs[i] = 0.0;
    }
  }

  for(size_t i = 0; i < 4; i++)
  {
    sum[0] += shapeFunctions[i] * values[i];
    sum[1] += shapeFunctions[4 + i] * values[i];
    sum[2] += shapeFunctions[8 + i] * values[i];
  }

  derivs[0] = sum[0] * jMatI(0, 0) + sum[1] * jMatI(0, 1) + sum[2] * jMatI(0, 2);
  derivs[1] = sum[0] * jMatI(1, 0) + sum[1] * jMatI(1, 1) + sum[2] * jMatI(1, 2);
  derivs[2] = sum[0] * jMatI(2, 0) + sum[1] * jMatI(2, 1) + sum[2] * jMatI(2, 2);
}

// -----------------------------------------------------------------------------
//
// -----------------------------------------------------------------------------
void DerivativeHelpers::HexDeriv::operator()(HexahedralGeom* hexas, size_t hexId, double values[8], double derivs[3])
{
  double shapeFunctions[24] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
  size_t verts[8] = {0, 0, 0, 0, 0, 0, 0, 0};
  double sum[3] = {0.0, 0.0, 0.0};
  double pCoords[3] = {0.0, 0.0, 0.0};

  hexas->getParametricCenter(pCoords);
  hexas->getShapeFunctions(pCoords, shapeFunctions);

  // Compute 3x3 Jacobian from vertex coordinates and tet shape functions,
  // then find the inverse Jacobian using Eigen
  double jPtr[9] = {
      0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
  };

  hexas->getVertsAtHex(hexId, verts);

  for(size_t i = 0; i < 8; i++)
  {
    float tmpCoords_f[3] = {0.0, 0.0, 0.0};
    double tmpCoords[3] = {0.0, 0.0, 0.0};
    hexas->getCoords(verts[i], tmpCoords_f);
    std::copy(tmpCoords_f, tmpCoords_f + 3, tmpCoords);
    for(size_t j = 0; j < 3; j++)
    {
      jPtr[j] += tmpCoords[j] * shapeFunctions[i];
      jPtr[j + 3] += tmpCoords[j] * shapeFunctions[8 + i];
      jPtr[j + 6] += tmpCoords[j] * shapeFunctions[16 + i];
    }
  }

  Eigen::Map<HexJacobian> jMat(jPtr);
  HexJacobian jMatI;
  bool invertible = false;

  jMat.computeInverseWithCheck(jMatI, invertible);

  // Jacobian is non-constant for a tet, so must check if inverse exists
  // If the Jacobian is not invertible, set derivatives to 0
  if(!invertible)
  {
    for(size_t i = 0; i < 3; i++)
    {
      derivs[i] = 0.0;
    }
  }

  for(size_t i = 0; i < 8; i++)
  {
    sum[0] += shapeFunctions[i] * values[i];
    sum[1] += shapeFunctions[8 + i] * values[i];
    sum[2] += shapeFunctions[16 + i] * values[i];
  }

  derivs[0] = sum[0] * jMatI(0, 0) + sum[1] * jMatI(0, 1) + sum[2] * jMatI(0, 2);
  derivs[1] = sum[0] * jMatI(1, 0) + sum[1] * jMatI(1, 1) + sum[2] * jMatI(1, 2);
  derivs[2] = sum[0] * jMatI(2, 0) + sum[1] * jMatI(2, 1) + sum[2] * jMatI(2, 2);
}