#include <trajopt_common/macros.h>
TRAJOPT_IGNORE_WARNINGS_PUSH
#include <boost/format.hpp>
#include <algorithm>
#include <cmath>
#include <chrono>
#include <cstdio>
#include <memory>
TRAJOPT_IGNORE_WARNINGS_POP

#include <trajopt_sco/expr_ops.hpp>
#include <trajopt_sco/modeling.hpp>
#include <trajopt_sco/optimizers.hpp>
#include <trajopt_sco/sco_common.hpp>
#include <trajopt_sco/solver_interface.hpp>
#include <tesseract/common/logging.h>
#include <trajopt_common/stl_to_string.hpp>
#include <tesseract/common/utils.h>

namespace sco
{

const bool SUPER_DEBUG_MODE = false;

std::string toString(OptStatus status)
{
  switch (status)
  {
    case OptStatus::OPT_CONVERGED:
      return "OPT_CONVERGED";
    case OptStatus::OPT_SCO_ITERATION_LIMIT:
      return "OPT_SCO_ITERATION_LIMIT";
    case OptStatus::OPT_PENALTY_ITERATION_LIMIT:
      return "OPT_PENALTY_ITERATION_LIMIT";
    case OptStatus::OPT_TIME_LIMIT:
      return "OPT_TIME_LIMIT";
    case OptStatus::OPT_FAILED:
      return "OPT_FAILED";
    case OptStatus::INVALID:
      return "INVALID";
    default:
      return "OPT_STATUS_UNKNOWN";
  }
}

std::ostream& operator<<(std::ostream& o, const OptResults& r)
{
  o << "Optimization results:" << '\n'
    << "status: " << toString(r.status) << '\n'
    << "cost values: " << trajopt_common::Str(r.cost_vals) << '\n'
    << "constraint violations: " << trajopt_common::Str(r.cnt_viols) << '\n'
    << "n func evals: " << r.n_func_evals << '\n'
    << "n qp solves: " << r.n_qp_solves << '\n';
  return o;
}

namespace
{
struct FileCloser
{
  void operator()(std::FILE* stream) const { std::fclose(stream); }
};
using FilePtr = std::unique_ptr<std::FILE, FileCloser>;

// todo: use different coeffs for each constraint
std::vector<ConvexObjective::Ptr> cntsToCosts(const std::vector<ConvexConstraints::Ptr>& cnts,
                                              const std::vector<double>& err_coeffs,
                                              Model* model)
{
  assert(cnts.size() == err_coeffs.size());
  std::vector<ConvexObjective::Ptr> out;
  for (std::size_t c = 0; c < cnts.size(); ++c)
  {
    auto obj = std::make_shared<ConvexObjective>(model);
    for (std::size_t idx = 0; idx < cnts[c]->eqs_.size(); ++idx)
    {
      const AffExpr& aff = cnts[c]->eqs_[idx];
      obj->addAbs(aff, err_coeffs[c]);
    }
    for (std::size_t idx = 0; idx < cnts[c]->ineqs_.size(); ++idx)
    {
      const AffExpr& aff = cnts[c]->ineqs_[idx];
      obj->addHinge(aff, err_coeffs[c]);
    }
    out.push_back(obj);
  }
  return out;
}
}  // namespace

bool BasicTrustRegionSQPParameters::operator==(const BasicTrustRegionSQPParameters& rhs) const
{
  static auto max_diff = static_cast<double>(std::numeric_limits<float>::epsilon());

  bool equal = true;
  equal &= tesseract::common::almostEqualRelativeAndAbs(improve_ratio_threshold, rhs.improve_ratio_threshold, max_diff);
  equal &= tesseract::common::almostEqualRelativeAndAbs(min_trust_box_size, rhs.min_trust_box_size, max_diff);
  equal &= tesseract::common::almostEqualRelativeAndAbs(min_approx_improve, rhs.min_approx_improve, max_diff);
  equal &= tesseract::common::almostEqualRelativeAndAbs(min_approx_improve_frac, rhs.min_approx_improve_frac, max_diff);
  equal &= (max_iter == rhs.max_iter);
  equal &= tesseract::common::almostEqualRelativeAndAbs(trust_shrink_ratio, rhs.trust_shrink_ratio, max_diff);
  equal &= tesseract::common::almostEqualRelativeAndAbs(trust_expand_ratio, rhs.trust_expand_ratio, max_diff);
  equal &= tesseract::common::almostEqualRelativeAndAbs(cnt_tolerance, rhs.cnt_tolerance, max_diff);
  equal &=
      tesseract::common::almostEqualRelativeAndAbs(max_merit_coeff_increases, rhs.max_merit_coeff_increases, max_diff);
  equal &= (max_qp_solver_failures == rhs.max_qp_solver_failures);
  equal &= tesseract::common::almostEqualRelativeAndAbs(
      merit_coeff_increase_ratio, rhs.merit_coeff_increase_ratio, max_diff);
  equal &= tesseract::common::almostEqualRelativeAndAbs(max_time, rhs.max_time, max_diff);
  equal &=
      tesseract::common::almostEqualRelativeAndAbs(initial_merit_error_coeff, rhs.initial_merit_error_coeff, max_diff);
  equal &= (inflate_constraints_individually == rhs.inflate_constraints_individually);
  equal &= tesseract::common::almostEqualRelativeAndAbs(trust_box_size, rhs.trust_box_size, max_diff);
  equal &= (log_results == rhs.log_results);
  equal &= (log_dir == rhs.log_dir);
  equal &= (num_threads == rhs.num_threads);
  return equal;
}

bool BasicTrustRegionSQPParameters::operator!=(const BasicTrustRegionSQPParameters& rhs) const
{
  return !operator==(rhs);
}

void Optimizer::addCallback(const Callback& cb) { callbacks_.push_back(cb); }
void Optimizer::callCallbacks()
{
  for (auto& callback : callbacks_)
  {
    callback(prob_.get(), results_);
  }
}

void Optimizer::initialize(const DblVec& x)
{
  if (!prob_)
    PRINT_AND_THROW("need to set the problem before initializing");
  if (prob_->getVars().size() != x.size())
    PRINT_AND_THROW(boost::format("initialization vector has wrong length. expected %i got %i") %
                    prob_->getVars().size() % x.size());
  results_.clear();
  results_.x = x;
}

BasicTrustRegionSQP::BasicTrustRegionSQP(const OptProb::Ptr& prob) { ctor(prob); }
void BasicTrustRegionSQP::setProblem(OptProb::Ptr prob) { ctor(prob); }
void BasicTrustRegionSQP::setParameters(const BasicTrustRegionSQPParameters& param) { param_ = param; }
const BasicTrustRegionSQPParameters& BasicTrustRegionSQP::getParameters() const { return param_; }
BasicTrustRegionSQPParameters& BasicTrustRegionSQP::getParameters() { return param_; }
void BasicTrustRegionSQP::ctor(const OptProb::Ptr& prob)
{
  if (!prob)
    PRINT_AND_THROW("the optimization problem is null");
  Optimizer::setProblem(prob);
  model_ = prob->getModel();
}

void BasicTrustRegionSQP::adjustTrustRegion(double ratio) { setTrustRegionSize(param_.trust_box_size * ratio); }
void BasicTrustRegionSQP::setTrustRegionSize(double trust_box_size) { param_.trust_box_size = trust_box_size; }
void BasicTrustRegionSQP::setTrustBoxConstraints(const DblVec& x)
{
  const VarVector& vars = prob_->getVars();
  assert(vars.size() == x.size());
  const DblVec& lb = prob_->getLowerBounds();
  const DblVec& ub = prob_->getUpperBounds();
  DblVec lbtrust(x.size());
  DblVec ubtrust(x.size());
  // Calculate box constraints, clamped to variable bounds. The iterate is first clamped into
  // [lb, ub] so the box stays non-empty when x has drifted outside its bounds (failed step, bad
  // warm-start); for x inside [lb, ub] this is a no-op and the box is the standard strict-shrink
  // trust region.
  for (std::size_t i = 0; i < x.size(); ++i)
  {
    const double xi = std::clamp(x[i], lb[i], ub[i]);
    lbtrust[i] = std::max(xi - param_.trust_box_size, lb[i]);
    ubtrust[i] = std::min(xi + param_.trust_box_size, ub[i]);
  }
  model_->setVarBounds(vars, lbtrust, ubtrust);
}

//////////////////////////////////////////////////
////// protected utility functions for  sqp //////
//////////////////////////////////////////////////

DblVec BasicTrustRegionSQP::evaluateCosts(const std::vector<Cost::Ptr>& costs, const DblVec& x) const
{
  DblVec out(costs.size());
  for (std::size_t i = 0; i < costs.size(); ++i)
    out[i] = costs[i]->value(x);

  return out;
}

DblVec BasicTrustRegionSQP::evaluateConstraintViols(const std::vector<Constraint::Ptr>& cnts, const DblVec& x) const
{
  DblVec out(cnts.size());
  for (std::size_t i = 0; i < cnts.size(); ++i)
    out[i] = cnts[i]->violation(x);

  return out;
}

std::vector<ConvexObjective::Ptr> BasicTrustRegionSQP::convexifyCosts(const std::vector<Cost::Ptr>& costs,
                                                                      const DblVec& x,
                                                                      Model* model) const
{
  std::vector<ConvexObjective::Ptr> out(costs.size());
  for (std::size_t i = 0; i < costs.size(); ++i)
    out[i] = costs[i]->convex(x, model);

  return out;
}

std::vector<ConvexConstraints::Ptr> BasicTrustRegionSQP::convexifyConstraints(const std::vector<Constraint::Ptr>& cnts,
                                                                              const DblVec& x,
                                                                              Model* model) const
{
  std::vector<ConvexConstraints::Ptr> out(cnts.size());
  for (std::size_t i = 0; i < cnts.size(); ++i)
    out[i] = cnts[i]->convex(x, model);

  return out;
}

DblVec BasicTrustRegionSQP::evaluateModelCosts(const std::vector<ConvexObjective::Ptr>& costs, const DblVec& x) const
{
  DblVec out(costs.size());
  for (std::size_t i = 0; i < costs.size(); ++i)
    out[i] = costs[i]->value(x);

  return out;
}

DblVec BasicTrustRegionSQP::evaluateModelCntViols(const std::vector<ConvexConstraints::Ptr>& cnts,
                                                  const DblVec& x) const
{
  DblVec out(cnts.size());
  for (std::size_t i = 0; i < cnts.size(); ++i)
    out[i] = cnts[i]->violation(x);

  return out;
}

std::vector<std::string> BasicTrustRegionSQP::getCostNames(const std::vector<Cost::Ptr>& costs) const
{
  std::vector<std::string> out(costs.size());
  for (std::size_t i = 0; i < costs.size(); ++i)
    out[i] = costs[i]->name();

  return out;
}

std::vector<std::string> BasicTrustRegionSQP::getCntNames(const std::vector<Constraint::Ptr>& cnts) const
{
  std::vector<std::string> out(cnts.size());
  for (std::size_t i = 0; i < cnts.size(); ++i)
    out[i] = cnts[i]->name();
  return out;
}

std::vector<std::string> BasicTrustRegionSQP::getVarNames(const VarVector& vars) const
{
  std::vector<std::string> out;
  out.reserve(vars.size());
  for (const auto& var : vars)
    out.push_back(var.var_rep->name);
  return out;
}

DblVec BasicTrustRegionSQPMultiThreaded::evaluateCosts(const std::vector<Cost::Ptr>& costs, const DblVec& x) const
{
  DblVec out(costs.size());
#pragma omp parallel for schedule(dynamic) num_threads(param_.num_threads) shared(out, costs, x)
  for (int i = 0; i < static_cast<int>(costs.size()); ++i)
  {
    out[static_cast<std::size_t>(i)] = costs[static_cast<std::size_t>(i)]->value(x);
  }

  return out;
}

DblVec BasicTrustRegionSQPMultiThreaded::evaluateConstraintViols(const std::vector<Constraint::Ptr>& cnts,
                                                                 const DblVec& x) const
{
  DblVec out(cnts.size());
#pragma omp parallel for schedule(dynamic) num_threads(param_.num_threads) shared(out, cnts, x)
  for (int i = 0; i < static_cast<int>(cnts.size()); ++i)
  {
    out[static_cast<std::size_t>(i)] = cnts[static_cast<std::size_t>(i)]->violation(x);
  }

  return out;
}

std::vector<ConvexObjective::Ptr> BasicTrustRegionSQPMultiThreaded::convexifyCosts(const std::vector<Cost::Ptr>& costs,
                                                                                   const DblVec& x,
                                                                                   Model* model) const
{
  std::vector<ConvexObjective::Ptr> out(costs.size());
#pragma omp parallel for schedule(dynamic) num_threads(param_.num_threads) shared(out, costs, x, model)
  for (int i = 0; i < static_cast<int>(costs.size()); ++i)
  {
    out[static_cast<std::size_t>(i)] = costs[static_cast<std::size_t>(i)]->convex(x, model);
  }

  return out;
}

std::vector<ConvexConstraints::Ptr>
BasicTrustRegionSQPMultiThreaded::convexifyConstraints(const std::vector<Constraint::Ptr>& cnts,
                                                       const DblVec& x,
                                                       Model* model) const
{
  std::vector<ConvexConstraints::Ptr> out(cnts.size());
#pragma omp parallel for schedule(dynamic) num_threads(param_.num_threads) shared(out, cnts, x, model)
  for (int i = 0; i < static_cast<int>(cnts.size()); ++i)
  {
    out[static_cast<std::size_t>(i)] = cnts[static_cast<std::size_t>(i)]->convex(x, model);
  }

  return out;
}

DblVec BasicTrustRegionSQPMultiThreaded::evaluateModelCosts(const std::vector<ConvexObjective::Ptr>& costs,
                                                            const DblVec& x) const
{
  DblVec out(costs.size());
#pragma omp parallel for schedule(dynamic) num_threads(param_.num_threads) shared(out, costs, x)
  for (int i = 0; i < static_cast<int>(costs.size()); ++i)
  {
    out[static_cast<std::size_t>(i)] = costs[static_cast<std::size_t>(i)]->value(x);
  }

  return out;
}

DblVec BasicTrustRegionSQPMultiThreaded::evaluateModelCntViols(const std::vector<ConvexConstraints::Ptr>& cnts,
                                                               const DblVec& x) const
{
  DblVec out(cnts.size());
#pragma omp parallel for schedule(dynamic) num_threads(param_.num_threads) shared(out, cnts, x)
  for (int i = 0; i < static_cast<int>(cnts.size()); ++i)
  {
    out[static_cast<std::size_t>(i)] = cnts[static_cast<std::size_t>(i)]->violation(x);
  }

  return out;
}

#if 0
struct MultiCritFilter {
  /**
   * Checks if you're making an improvement on a multidimensional objective
   * Given a set of past error vectors, the improvement is defined as
   * min_{olderrvec in past_err_vecs} | olderrvec - errvec |^+
   */
  vector<DblVec> errvecs;
  double improvement(const DblVec& errvec) {
    double leastImprovement=INFINITY;
    for (const DblVec& olderrvec : errvecs) {
      double improvement=0;
      for (int i=0; i < errvec.size(); ++i) improvement += pospart(olderrvec[i] - errvec[i]);
      leastImprovement = fmin(leastImprovement, improvement);
    }
    return leastImprovement;
  }
  void insert(const DblVec& x) {errvecs.push_back(x);}
  bool empty() {return errvecs.size() > 0;}
};
#endif

BasicTrustRegionSQPResults::BasicTrustRegionSQPResults(std::vector<std::string> var_names,
                                                       std::vector<std::string> cost_names,
                                                       std::vector<std::string> cnt_names,
                                                       const BasicTrustRegionSQP& parent)
  : var_names(std::move(var_names)), cost_names(std::move(cost_names)), cnt_names(std::move(cnt_names)), parent_(parent)
{
  model_var_vals.clear();
  model_cost_vals.clear();
  model_cnt_viols.clear();
  new_x.clear();
  new_cost_vals.clear();
  old_cost_vals.clear();
  new_cnt_viols.clear();
  old_cnt_viols.clear();
  merit_error_coeffs = std::vector<double>(this->cnt_names.size(), 0);
}

void BasicTrustRegionSQPResults::update(const OptResults& prev_opt_results,
                                        const Model& model,
                                        const std::vector<ConvexObjective::Ptr>& cost_models,
                                        const std::vector<ConvexConstraints::Ptr>& cnt_models,
                                        const std::vector<ConvexObjective::Ptr>& cnt_cost_models,
                                        const std::vector<Constraint::Ptr>& constraints,
                                        const std::vector<Cost::Ptr>& costs,
                                        std::vector<double> merit_error_coeffs)
{
  this->merit_error_coeffs = merit_error_coeffs;
  model_var_vals = model.getVarValues(model.getVars());
  model_cost_vals = parent_.evaluateModelCosts(cost_models, model_var_vals);
  model_cnt_viols = parent_.evaluateModelCntViols(cnt_models, model_var_vals);

  // the n variables of the OptProb happen to be the first n variables in
  // the Model
  new_x = DblVec(model_var_vals.begin(), model_var_vals.begin() + static_cast<long int>(prev_opt_results.x.size()));

  if (tesseract::common::isLogLevelEnabled(spdlog::level::debug))
  {
    const DblVec cnt_costs1 = parent_.evaluateModelCosts(cnt_cost_models, model_var_vals);
    DblVec cnt_costs2 = model_cnt_viols;
    for (unsigned i = 0; i < cnt_costs2.size(); ++i)
      cnt_costs2[i] *= merit_error_coeffs[i];
    TESSERACT_LOG_DEBUG("SHOULD BE ALMOST THE SAME: {} ?= {}", CSTR(cnt_costs1), CSTR(cnt_costs2));
    // not exactly the same because cnt_costs1 is based on aux variables,
    // but they might not be at EXACTLY the right value
  }

  old_cost_vals = prev_opt_results.cost_vals;
  old_cnt_viols = prev_opt_results.cnt_viols;
  new_cost_vals = parent_.evaluateCosts(costs, new_x);
  new_cnt_viols = parent_.evaluateConstraintViols(constraints, new_x);

  old_merit = vecSum(old_cost_vals) + vecDot(old_cnt_viols, merit_error_coeffs);
  model_merit = vecSum(model_cost_vals) + vecDot(model_cnt_viols, merit_error_coeffs);
  new_merit = vecSum(new_cost_vals) + vecDot(new_cnt_viols, merit_error_coeffs);
  approx_merit_improve = old_merit - model_merit;
  exact_merit_improve = old_merit - new_merit;
  merit_improve_ratio = exact_merit_improve / approx_merit_improve;

  if (tesseract::common::isLogLevelEnabled(spdlog::level::info))
  {
    TESSERACT_LOG_INFO(" ");
    print();
  }
}

void BasicTrustRegionSQPResults::print() const
{
  // Print Header
  std::printf("\n| %s |\n", std::string(88, '=').c_str());
  std::printf("| %s %s %s |\n", std::string(36, ' ').c_str(), "ROS Industrial", std::string(36, ' ').c_str());
  std::printf("| %s %s %s |\n", std::string(32, ' ').c_str(), "TrajOpt Motion Planning", std::string(31, ' ').c_str());
  std::printf("| %s |\n", std::string(88, '=').c_str());

  // Print Cost and Constraint Data
  std::printf("| %10s | %10s | %10s | %10s | %10s | %10s | %10s |\n",
              "merit",
              "oldexact",
              "new_exact",
              "new_approx",
              "dapprox",
              "dexact",
              "ratio");
  std::printf("| %s | COSTS\n", std::string(88, '-').c_str());
  for (std::size_t i = 0; i < old_cost_vals.size(); ++i)
  {
    const double approx_improve = old_cost_vals[i] - model_cost_vals[i];
    const double exact_improve = old_cost_vals[i] - new_cost_vals[i];
    if (fabs(approx_improve) > 1e-8)
      std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s \n",
                  "----------",
                  old_cost_vals[i],
                  new_cost_vals[i],
                  model_cost_vals[i],
                  approx_improve,
                  exact_improve,
                  exact_improve / approx_improve,
                  cost_names[i].c_str());
    else
      std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10s | %-15s \n",
                  "----------",
                  old_cost_vals[i],
                  new_cost_vals[i],
                  model_cost_vals[i],
                  approx_improve,
                  exact_improve,
                  "----------",
                  cost_names[i].c_str());
  }
  std::printf("| %s |\n", std::string(88, '=').c_str());
  std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10s | %10s | %10s | SUM COSTS\n",
              "----------",
              vecSum(old_cost_vals),
              vecSum(new_cost_vals),
              vecSum(model_cost_vals),
              "----------",
              "----------",
              "----------");
  std::printf("| %s |\n", std::string(88, '=').c_str());

  if (!cnt_names.empty())
  {
    std::printf("| %s | CONSTRAINTS\n", std::string(88, '-').c_str());
    for (std::size_t i = 0; i < old_cnt_viols.size(); ++i)
    {
      const double approx_improve = old_cnt_viols[i] - model_cnt_viols[i];
      const double exact_improve = old_cnt_viols[i] - new_cnt_viols[i];
      if (fabs(approx_improve) > 1e-8)
        std::printf("| %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %-15s \n",
                    merit_error_coeffs[i],
                    merit_error_coeffs[i] * old_cnt_viols[i],
                    merit_error_coeffs[i] * new_cnt_viols[i],
                    merit_error_coeffs[i] * model_cnt_viols[i],
                    merit_error_coeffs[i] * approx_improve,
                    merit_error_coeffs[i] * exact_improve,
                    exact_improve / approx_improve,
                    cnt_names[i].c_str());
      else
        std::printf("| %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10.3e | %10s | %-15s \n",
                    merit_error_coeffs[i],
                    merit_error_coeffs[i] * old_cnt_viols[i],
                    merit_error_coeffs[i] * new_cnt_viols[i],
                    merit_error_coeffs[i] * model_cnt_viols[i],
                    merit_error_coeffs[i] * approx_improve,
                    merit_error_coeffs[i] * exact_improve,
                    "----------",
                    cnt_names[i].c_str());
    }
  }
  std::printf("| %s |\n", std::string(88, '=').c_str());
  std::printf("| %10s | %10.3e | %10.3e | %10.3e | %10s | %10s | %10s | SUM CONSTRAINTS (WITHOUT MERIT) \n",
              "----------",
              vecSum(old_cnt_viols),
              vecSum(new_cnt_viols),
              vecSum(model_cnt_viols),
              "----------",
              "----------",
              "----------");
  std::printf("| %s |\n", std::string(88, '=').c_str());
  std::printf("| %10s | %10.3e | %10.3e | %10s | %10.3e | %10.3e | %10.3e | TOTAL = SUM COSTS + SUM CONSTRAINTS (WITH "
              "MERIT)\n",
              "----------",
              old_merit,
              new_merit,
              "----------",
              approx_merit_improve,
              exact_merit_improve,
              merit_improve_ratio);
  std::printf("| %s |\n", std::string(88, '=').c_str());
}

void BasicTrustRegionSQPResults::writeSolver(std::FILE* stream, bool header) const
{
  if (header)
    std::fprintf(stream, "%s,%s,%s,%s,%s,%s\n", "DESCRIPTION", "oldexact", "new_exact", "dapprox", "dexact", "ratio");

  std::fprintf(stream,
               "%s,%10.3e,%10.3e,%10.3e,%10.3e,%10.3e\n",
               "Solver",
               old_merit,
               new_merit,
               approx_merit_improve,
               exact_merit_improve,
               merit_improve_ratio);
  std::fflush(stream);
}

void BasicTrustRegionSQPResults::writeVars(std::FILE* stream, bool header) const
{
  if (header)
  {
    std::fprintf(stream, "%s", "NAMES");
    for (const auto& var : var_names)
      std::fprintf(stream, ",%s", var.c_str());

    std::fprintf(stream, "\n");
  }

  std::fprintf(stream, "%s", "VALUES");
  for (const auto& x : new_x)
    std::fprintf(stream, ",%e", x);

  std::fprintf(stream, "\n");
  std::fflush(stream);
}

void BasicTrustRegionSQPResults::writeCosts(std::FILE* stream, bool header) const
{
  if (header)
  {
    std::fprintf(stream, "%s", "COST NAMES");
    for (const auto& name : cost_names)
      std::fprintf(stream, ",%s,%s,%s,%s", name.c_str(), name.c_str(), name.c_str(), name.c_str());

    std::fprintf(stream, "\n");

    std::fprintf(stream, "%s", "DESCRIPTION");
    for (std::size_t i = 0; i < cost_names.size(); ++i)
      std::fprintf(stream, ",%s,%s,%s,%s", "oldexact", "dapprox", "dexact", "ratio");

    std::fprintf(stream, "\n");
  }

  std::fprintf(stream, "%s", "COSTS");
  for (std::size_t i = 0; i < old_cost_vals.size(); ++i)
  {
    const double approx_improve = old_cost_vals[i] - model_cost_vals[i];
    const double exact_improve = old_cost_vals[i] - new_cost_vals[i];
    if (fabs(approx_improve) > 1e-8)
    {
      std::fprintf(
          stream, ",%e,%e,%e,%e", old_cost_vals[i], approx_improve, exact_improve, exact_improve / approx_improve);
    }
    else
    {
      std::fprintf(stream, ",%e,%e,%e,%s", old_cost_vals[i], approx_improve, exact_improve, "nan");
    }
  }
  std::fprintf(stream, "\n");
  std::fflush(stream);
}

void BasicTrustRegionSQPResults::writeConstraints(std::FILE* stream, bool header) const
{
  if (header)
  {
    std::fprintf(stream, "%s", "CONSTRAINT NAMES");
    for (const auto& name : cnt_names)
      std::fprintf(stream, ",%s,%s,%s,%s", name.c_str(), name.c_str(), name.c_str(), name.c_str());

    std::fprintf(stream, "\n");

    std::fprintf(stream, "%s", "DESCRIPTION");
    for (std::size_t i = 0; i < cnt_names.size(); ++i)
      std::fprintf(stream, ",%s,%s,%s,%s", "oldexact", "dapprox", "dexact", "ratio");

    std::fprintf(stream, "\n");
  }

  std::fprintf(stream, "%s", "CONSTRAINTS");
  for (std::size_t i = 0; i < old_cnt_viols.size(); ++i)
  {
    const double approx_improve = old_cnt_viols[i] - model_cnt_viols[i];
    const double exact_improve = old_cnt_viols[i] - new_cnt_viols[i];
    if (fabs(approx_improve) > 1e-8)
    {
      std::fprintf(stream,
                   ",%e,%e,%e,%e",
                   merit_error_coeffs[i] * old_cnt_viols[i],
                   merit_error_coeffs[i] * approx_improve,
                   merit_error_coeffs[i] * exact_improve,
                   exact_improve / approx_improve);
    }
    else
    {
      std::fprintf(stream,
                   ",%e,%e,%e,%s",
                   merit_error_coeffs[i] * old_cnt_viols[i],
                   merit_error_coeffs[i] * approx_improve,
                   merit_error_coeffs[i] * exact_improve,
                   "nan");
    }
  }
  std::fprintf(stream, "\n");
  std::fflush(stream);
}

void BasicTrustRegionSQPResults::printRaw() const
{
  std::cout << "\nmodel_var_vals:";
  for (const auto& i : model_var_vals)
    std::cout << i << ", ";

  std::cout << "\nmodel_cost_vals: ";
  for (const auto& i : model_cost_vals)
    std::cout << i << ", ";

  std::cout << "\nmodel_cnt_viols: ";
  for (const auto& i : model_cnt_viols)
    std::cout << i << ", ";
  std::cout << "\nnew_x: ";
  for (const auto& i : new_x)
    std::cout << i << ", ";
  std::cout << "\nnew_cost_vals: ";
  for (const auto& i : new_cost_vals)
    std::cout << i << ", ";
  std::cout << "\nold_cost_vals: ";
  for (const auto& i : old_cost_vals)
    std::cout << i << ", ";
  std::cout << "\nnew_cnt_viols: ";
  for (const auto& i : new_cnt_viols)
    std::cout << i << ", ";
  std::cout << "\nold_cnt_viols: ";
  for (const auto& i : old_cnt_viols)
    std::cout << i << ", ";

  std::cout << "\nold_merit: " << old_merit << " \n";
  std::cout << "model_merit: " << model_merit << " \n";
  std::cout << "new_merit: " << new_merit << " \n";
  std::cout << "approx_merit_improve: " << approx_merit_improve << " \n";
  std::cout << "exact_merit_improve: " << exact_merit_improve << " \n";
  std::cout << "merit_improve_ratio: " << merit_improve_ratio << " \n";

  std::cout << "merit_error_coeffs: ";
  for (const auto& i : merit_error_coeffs)
    std::cout << i << ", ";
  std::cout << "\nvar_names: ";
  for (const auto& i : var_names)
    std::cout << i << ", ";
  std::cout << "\ncost_names: ";
  for (const auto& i : cost_names)
    std::cout << i << ", ";
  std::cout << "\ncnt_names: ";
  for (const auto& i : cnt_names)
    std::cout << i << ", ";
}

OptStatus BasicTrustRegionSQP::optimize()
{
  if (!prob_)
    PRINT_AND_THROW("you forgot to set the optimization problem");
  if (results_.x.empty())
    PRINT_AND_THROW("you forgot to initialize!");

  const std::vector<std::string> var_names = getVarNames(prob_->getVars());
  const std::vector<std::string> cost_names = getCostNames(prob_->getCosts());
  const std::vector<Constraint::Ptr> constraints = prob_->getConstraints();
  std::vector<std::string> cnt_names = getCntNames(constraints);
  std::vector<double> merit_error_coeffs(constraints.size(), param_.initial_merit_error_coeff);
  BasicTrustRegionSQPResults iteration_results(var_names, cost_names, cnt_names, *this);

  FilePtr log_solver_stream;
  FilePtr log_vars_stream;
  FilePtr log_costs_stream;
  FilePtr log_constraints_stream;
  if (param_.log_results || tesseract::common::isLogLevelEnabled(spdlog::level::debug))
  {
    log_solver_stream.reset(std::fopen((param_.log_dir + "/trajopt_solver.log").c_str(), "w"));
    log_vars_stream.reset(std::fopen((param_.log_dir + "/trajopt_vars.log").c_str(), "w"));
    log_costs_stream.reset(std::fopen((param_.log_dir + "/trajopt_costs.log").c_str(), "w"));
    log_constraints_stream.reset(std::fopen((param_.log_dir + "/trajopt_constraints.log").c_str(), "w"));
  }

  results_.x = prob_->getClosestFeasiblePoint(results_.x);

  assert(results_.x.size() == prob_->getVars().size());
  assert(!prob_->getCosts().empty() || !constraints.empty());

  OptStatus retval = INVALID;

  using Clock = std::chrono::high_resolution_clock;
  auto start_time = Clock::now();

  for (int merit_increases = 0; merit_increases < param_.max_merit_coeff_increases; ++merit_increases)
  { /* merit adjustment loop */
    for (int iter = 1;; ++iter)
    { /* sqp loop */
      const double elapsed_time = std::chrono::duration<double, std::milli>(Clock::now() - start_time).count() / 1000.0;
      if (elapsed_time > param_.max_time)
      {
        TESSERACT_LOG_INFO("Elapsed time {} has exceeded max time {}", elapsed_time, param_.max_time);
        retval = OPT_TIME_LIMIT;

        if (results_.cnt_viols.empty() || vecMax(results_.cnt_viols) < param_.cnt_tolerance)
        {
          retval = OPT_CONVERGED;
          if (!results_.cnt_viols.empty())
            TESSERACT_LOG_INFO("woo-hoo! constraints are satisfied (to tolerance {:.2e})", param_.cnt_tolerance);
        }

        goto cleanup;
      }
      callCallbacks();

      if (tesseract::common::isLogLevelEnabled(spdlog::level::debug))
        TESSERACT_LOG_DEBUG("current iterate: {}", CSTR(results_.x));
      TESSERACT_LOG_INFO("iteration {}", iter);

      // speedup: if you just evaluated the cost when doing the line search, use
      // that
      if (results_.cost_vals.empty() && results_.cnt_viols.empty())
      {  // only happens on the first iteration
        results_.cnt_viols = evaluateConstraintViols(constraints, results_.x);
        results_.cost_vals = evaluateCosts(prob_->getCosts(), results_.x);
        assert(results_.n_func_evals == 0);
        ++results_.n_func_evals;
      }

      // DblVec new_cnt_viols = evaluateConstraintViols(constraints, results_.x);
      // DblVec new_cost_vals = evaluateCosts(prob_->getCosts(), results_.x);
      // cout << "costs" << endl;
      // for (int i=0; i < new_cnt_viols.size(); ++i) {
      //   cout << cnt_names[i] << " " << new_cnt_viols[i] -
      //   results_.cnt_viols[i] << endl;
      // }
      // for (int i=0; i < new_cost_vals.size(); ++i) {
      //   cout << cost_names[i] << " " << new_cost_vals[i] -
      //   results_.cost_vals[i] << endl;
      // }

      const std::vector<ConvexObjective::Ptr> cost_models = convexifyCosts(prob_->getCosts(), results_.x, model_.get());
      const std::vector<ConvexConstraints::Ptr> cnt_models =
          convexifyConstraints(constraints, results_.x, model_.get());
      const std::vector<ConvexObjective::Ptr> cnt_cost_models =
          cntsToCosts(cnt_models, merit_error_coeffs, model_.get());
      model_->update();
      for (const ConvexObjective::Ptr& cost : cost_models)
        cost->addConstraintsToModel();
      for (const ConvexObjective::Ptr& cost : cnt_cost_models)
        cost->addConstraintsToModel();
      model_->update();
      QuadExpr objective;
      for (const ConvexObjective::Ptr& co : cost_models)
        exprInc(objective, co->quad_);
      for (const ConvexObjective::Ptr& co : cnt_cost_models)
        exprInc(objective, co->quad_);

      //    objective = cleanupExpr(objective);
      model_->setObjective(objective);

      int qp_solver_failures = 0;
      while (param_.trust_box_size >= param_.min_trust_box_size)
      {
        setTrustBoxConstraints(results_.x);
        const CvxOptStatus status = model_->optimize();

        ++results_.n_qp_solves;
        if (status != CVX_SOLVED)
        {
          TESSERACT_LOG_WARN("Convex solver failed. Enable debug logging to see solver output. Saving model to "
                             "/tmp/fail.lp");
          model_->writeToFile("/tmp/fail.lp");
          if (qp_solver_failures < (param_.max_qp_solver_failures - 1))
          {
            adjustTrustRegion(param_.trust_shrink_ratio);
            TESSERACT_LOG_INFO("shrunk trust region. new box size: {:.4f}", param_.trust_box_size);
            qp_solver_failures++;
            continue;
          }

          if (qp_solver_failures == (param_.max_qp_solver_failures - 1))
          {
            // convex solver failed and this is the last attempt so setting the trust region to the minimum.
            setTrustRegionSize(param_.min_trust_box_size);
            TESSERACT_LOG_INFO("shrunk trust region. new box size: {:.4f}", param_.trust_box_size);
            qp_solver_failures++;
            continue;
          }

          TESSERACT_LOG_ERROR("The convex solver failed you one too many times.");
          retval = OPT_FAILED;
          goto cleanup;
        }

        iteration_results.update(results_,
                                 *model_,
                                 cost_models,
                                 cnt_models,
                                 cnt_cost_models,
                                 constraints,
                                 prob_->getCosts(),
                                 merit_error_coeffs);
        if (SUPER_DEBUG_MODE)
        {
          model_->writeToFile("trajopt_model.txt");
          iteration_results.printRaw();
        }

        if (param_.log_results || tesseract::common::isLogLevelEnabled(spdlog::level::debug))
        {
          if (log_solver_stream != nullptr)
            iteration_results.writeSolver(log_solver_stream.get(), results_.n_func_evals == 1);

          if (log_vars_stream != nullptr)
            iteration_results.writeVars(log_vars_stream.get(), results_.n_func_evals == 1);

          if (log_costs_stream != nullptr)
            iteration_results.writeCosts(log_costs_stream.get(), results_.n_func_evals == 1);

          if (log_constraints_stream != nullptr)
            iteration_results.writeConstraints(log_constraints_stream.get(), results_.n_func_evals == 1);
        }

        ++results_.n_func_evals;

        if (iteration_results.approx_merit_improve < -1e-5)
        {
          TESSERACT_LOG_WARN("approximate merit function got worse ({:.3e}). "
                             "(convexification is probably wrong to zeroth order)",
                             iteration_results.approx_merit_improve);
        }

        if (iteration_results.approx_merit_improve < param_.min_approx_improve)
        {
          TESSERACT_LOG_INFO("converged because improvement was small ({:.3e} < {:.3e})",
                             iteration_results.approx_merit_improve,
                             param_.min_approx_improve);
          retval = OPT_CONVERGED;
          goto penaltyadjustment;
        }

        const double merit_denom = std::max(std::abs(iteration_results.old_merit), 1e-12);
        if (iteration_results.approx_merit_improve / merit_denom < param_.min_approx_improve_frac)
        {
          TESSERACT_LOG_INFO("converged because improvement ratio was small ({:.3e} < {:.3e})",
                             iteration_results.approx_merit_improve / merit_denom,
                             param_.min_approx_improve_frac);
          retval = OPT_CONVERGED;
          goto penaltyadjustment;
        }
        else if (iteration_results.exact_merit_improve < 0 ||
                 iteration_results.merit_improve_ratio < param_.improve_ratio_threshold)
        {
          adjustTrustRegion(param_.trust_shrink_ratio);
          TESSERACT_LOG_INFO("shrunk trust region. new box size: {:.4f}", param_.trust_box_size);
        }
        else
        {
          results_.x = iteration_results.new_x;
          results_.cost_vals = iteration_results.new_cost_vals;
          results_.cnt_viols = iteration_results.new_cnt_viols;
          adjustTrustRegion(param_.trust_expand_ratio);
          TESSERACT_LOG_INFO("expanded trust region. new box size: {:.4f}", param_.trust_box_size);
          break;
        }
      }

      if (param_.trust_box_size < param_.min_trust_box_size)
      {
        TESSERACT_LOG_INFO("converged because trust region is tiny");
        retval = OPT_CONVERGED;
        goto penaltyadjustment;
      }
      else if (iter >= param_.max_iter)
      {
        TESSERACT_LOG_INFO("iteration limit");
        retval = OPT_SCO_ITERATION_LIMIT;

        if (results_.cnt_viols.empty() || vecMax(results_.cnt_viols) < param_.cnt_tolerance)
        {
          retval = OPT_CONVERGED;
          if (!results_.cnt_viols.empty())
            TESSERACT_LOG_INFO("woo-hoo! constraints are satisfied (to tolerance {:.2e})", param_.cnt_tolerance);
        }

        goto cleanup;
      }
    } /* sqp loop */

  penaltyadjustment:
    if (results_.cnt_viols.empty() || vecMax(results_.cnt_viols) < param_.cnt_tolerance)
    {
      if (!results_.cnt_viols.empty())
        TESSERACT_LOG_INFO("woo-hoo! constraints are satisfied (to tolerance {:.2e})", param_.cnt_tolerance);
      goto cleanup;  // NOLINT
    }
    else
    {
      if (param_.inflate_constraints_individually)
      {
        assert(results_.cnt_viols.size() == merit_error_coeffs.size());
        for (std::size_t idx = 0; idx < results_.cnt_viols.size(); idx++)
        {
          if (results_.cnt_viols[idx] > param_.cnt_tolerance)
          {
            if (tesseract::common::isLogLevelEnabled(spdlog::level::debug))
              TESSERACT_LOG_DEBUG("Not all constraints are satisfied. Increasing constraint penalties for {}",
                                  CSTR(cnt_names[idx]));
            merit_error_coeffs[idx] *= param_.merit_coeff_increase_ratio;
          }
        }
      }
      else
      {
        TESSERACT_LOG_DEBUG("Not all constraints are satisfied. Increasing constraint penalties uniformly");
        for (auto& merit_error_coeff : merit_error_coeffs)
          merit_error_coeff *= param_.merit_coeff_increase_ratio;
      }
      if (tesseract::common::isLogLevelEnabled(spdlog::level::debug))
        TESSERACT_LOG_DEBUG("New merit_error_coeffs: {}", CSTR(merit_error_coeffs));
      param_.trust_box_size = fmax(param_.trust_box_size, param_.min_trust_box_size / param_.trust_shrink_ratio * 1.5);
    }
  } /* merit adjustment loop */
  retval = OPT_PENALTY_ITERATION_LIMIT;
  TESSERACT_LOG_INFO("optimization couldn't satisfy all constraints");

cleanup:
  assert(retval != INVALID && "should never happen");
  results_.status = retval;
  results_.total_cost = vecSum(results_.cost_vals);
  if (tesseract::common::isLogLevelEnabled(spdlog::level::info))
    TESSERACT_LOG_INFO("\n==================\n{}==================", CSTR(results_));
  callCallbacks();

  return retval;
}
}  // namespace sco