// Copyright (C) 2004, 2008 International Business Machines and others.
// All Rights Reserved.
// This code is published under the Common Public License.
//
// $Id: IpDefaultIterateInitializer.cpp 1219 2008-04-25 18:14:35Z andreasw $
//
// Authors:  Carl Laird, Andreas Waechter              IBM    2004-09-23

#include "IpDefaultIterateInitializer.hpp"

namespace Ipopt
{
#if COIN_IPOPT_VERBOSITY > 0
  static const Index dbg_verbosity = 0;
#endif

  DefaultIterateInitializer::DefaultIterateInitializer
  (const SmartPtr<EqMultiplierCalculator>& eq_mult_calculator,
   const SmartPtr<IterateInitializer>& warm_start_initializer,
   const SmartPtr<AugSystemSolver> aug_system_solver /*= NULL*/)
      :
      IterateInitializer(),
      eq_mult_calculator_(eq_mult_calculator),
      warm_start_initializer_(warm_start_initializer),
      aug_system_solver_(aug_system_solver)
  {}

  void DefaultIterateInitializer::RegisterOptions(SmartPtr<RegisteredOptions> reg_options)
  {
    reg_options->AddLowerBoundedNumberOption(
      "bound_push",
      "Desired minimum absolute distance from the initial point to bound.",
      0.0, true, 0.01,
      "Determines how much the initial point might have to "
      "be modified in order to be sufficiently inside "
      "the bounds (together with \"bound_frac\").  (This is kappa_1 in "
      "Section 3.6 of implementation paper.)");
    reg_options->AddBoundedNumberOption(
      "bound_frac",
      "Desired minimum relative distance from the initial point to bound.",
      0, true, 0.5, false, 0.01,
      "Determines how much the initial point might have to "
      "be modified in order to be sufficiently inside "
      "the bounds (together with \"bound_push\").  (This is kappa_2 in "
      "Section 3.6 of implementation paper.)");
    reg_options->AddLowerBoundedNumberOption(
      "slack_bound_push",
      "Desired minimum absolute distance from the initial slack to bound.",
      0.0, true, 0.01,
      "Determines how much the initial slack variables might have to "
      "be modified in order to be sufficiently inside "
      "the inequality bounds (together with \"slack_bound_frac\").  (This is kappa_1 in "
      "Section 3.6 of implementation paper.)");
    reg_options->AddBoundedNumberOption(
      "slack_bound_frac",
      "Desired minimum relative distance from the initial slack to bound.",
      0, true, 0.5, false, 0.01,
      "Determines how much the initial slack variables might have to "
      "be modified in order to be sufficiently inside "
      "the inequality bounds (together with \"slack_bound_push\").  (This is kappa_2 in "
      "Section 3.6 of implementation paper.)");
    reg_options->AddLowerBoundedNumberOption(
      "constr_mult_init_max",
      "Maximum allowed least-square guess of constraint multipliers.",
      0, false, 1e3,
      "Determines how large the initial least-square guesses of the constraint "
      "multipliers are allowed to be (in max-norm). If the guess is larger "
      "than this value, it is discarded and all constraint multipliers are "
      "set to zero.  This options is also used when initializing the "
      "restoration phase. By default, \"resto.constr_mult_init_max\" (the one "
      "used in RestoIterateInitializer) is set to zero.");
    reg_options->AddLowerBoundedNumberOption(
      "bound_mult_init_val",
      "Initial value for the bound multipliers.",
      0, true, 1.0,
      "All dual variables corresponding to bound constraints are "
      "initialized to this value.");
    reg_options->AddStringOption2(
      "bound_mult_init_method",
      "Initialization method for bound multipliers",
      "constant",
      "constant", "set all bound multipliers to the value of bound_mult_init_val",
      "mu-based", "initialize to mu_init/x_slack",
      "This option defines how the iterates for the bound multipliers are "
      "initialized.  If \"constant\" is chosen, then all bound multipliers "
      "are initialized to the value of \"bound_mult_init_val\".  If "
      "\"mu-based\" is chosen, the each value is initialized to the the value "
      "of \"mu_init\" divided by the corresponding slack variable.  This "
      "latter option might be useful if the starting point is close to the "
      "optimal solution.");
    reg_options->AddStringOption2(
      "least_square_init_primal",
      "Least square initialization of the primal variables", "no",
      "no", "take user-provided point",
      "yes", "overwrite user-provided point with least-square estimates",
      "If set to yes, Ipopt ignores the user provided point and solves a "
      "least square problem for the primal variables (x and s), to fit the "
      "linearized equality and inequality constraints.  This might be useful "
      "if the user doesn't know anything about the starting point, or for "
      "solving an LP or QP.");
    reg_options->AddStringOption2(
      "least_square_init_duals",
      "Least square initialization of all dual variables", "no",
      "no", "use bound_mult_init_val and least-square equality constraint multipliers",
      "yes", "overwrite user-provided point with least-square estimates",
      "If set to yes, Ipopt tries to compute least-square multipliers "
      "(considering ALL dual variables).  If successful, the bound "
      "multipliers are possibly corrected to be at least bound_mult_init_val. "
      "This might be useful "
      "if the user doesn't know anything about the starting point, or for "
      "solving an LP or QP.  This overwrites option "
      "\"bound_mult_init_method\".");
    reg_options->SetRegisteringCategory("Warm Start");
    reg_options->AddStringOption2(
      "warm_start_init_point",
      "Warm-start for initial point", "no",
      "no", "do not use the warm start initialization",
      "yes", "use the warm start initialization",
      "Indicates whether this optimization should use a warm start "
      "initialization, where values of primal and dual variables are "
      "given (e.g., from a previous optimization of a related problem.)");
  }

  bool DefaultIterateInitializer::InitializeImpl(const OptionsList& options,
      const std::string& prefix)
  {
    // Check for the algorithm options
    options.GetNumericValue("bound_push", bound_push_, prefix);
    options.GetNumericValue("bound_frac", bound_frac_, prefix);
    if (!options.GetNumericValue("slack_bound_push", slack_bound_push_, prefix)) {
      slack_bound_push_ = bound_push_;
    }
    if (!options.GetNumericValue("slack_bound_frac", slack_bound_frac_, prefix)) {
      slack_bound_frac_ = bound_frac_;
    }
    options.GetNumericValue("constr_mult_init_max",
                            constr_mult_init_max_, prefix);
    options.GetNumericValue("bound_mult_init_val",
                            bound_mult_init_val_, prefix);
    options.GetBoolValue("warm_start_init_point",
                         warm_start_init_point_, prefix);
    options.GetBoolValue("least_square_init_primal",
                         least_square_init_primal_, prefix);
    ASSERT_EXCEPTION(!least_square_init_primal_ || IsValid(aug_system_solver_),
                     OPTION_INVALID,
                     "The least_square_init_primal can only be chosen if the DefaultInitializer object has an AugSystemSolver.\n");
    options.GetBoolValue("least_square_init_duals",
                         least_square_init_duals_, prefix);
    ASSERT_EXCEPTION(!least_square_init_duals_ || IsValid(aug_system_solver_),
                     OPTION_INVALID,
                     "The least_square_init_duals can only be chosen if the DefaultInitializer object has an AugSystemSolver.\n");
    int enum_int;
    options.GetEnumValue("bound_mult_init_method", enum_int, prefix);
    bound_mult_init_method_ = BoundMultInitMethod(enum_int);
    if (bound_mult_init_method_ == B_MU_BASED) {
      options.GetNumericValue("mu_init",  mu_init_, prefix);
    }

    bool retvalue = true;
    if (IsValid(eq_mult_calculator_)) {
      retvalue = eq_mult_calculator_->Initialize(Jnlst(), IpNLP(), IpData(),
                 IpCq(), options, prefix);
      if (!retvalue) {
        return retvalue;
      }
    }
    if (IsValid(warm_start_initializer_)) {
      retvalue =
        warm_start_initializer_->Initialize(Jnlst(), IpNLP(), IpData(),
                                            IpCq(), options, prefix);
    }
    return retvalue;
  }

  bool DefaultIterateInitializer::SetInitialIterates()
  {
    DBG_START_METH("DefaultIterateInitializer::SetInitialIterates",
                   dbg_verbosity);

    if (warm_start_init_point_) {
      DBG_ASSERT(IsValid(warm_start_initializer_));
      return warm_start_initializer_->SetInitialIterates();
    }

    // Get the starting values provided by the NLP and store them
    // in the ip_data current fields.  The following line only requests
    // intial values for the primal variables x, but later we might
    // make this more flexible based on user options.

    /////////////////////////////////////////////////////////////////////
    //                   Initialize primal variables                   //
    /////////////////////////////////////////////////////////////////////

    if (!IpData().InitializeDataStructures(IpNLP(), true, false, false,
                                           false, false)) {
      return false;
    }

    // get a container of the current point. We will modify parts of
    // this IteratesVector to set the trial point.
    SmartPtr<IteratesVector> iterates = IpData().curr()->MakeNewContainer();
    if (least_square_init_primal_) {
      // If least_square_init_primal, then we compute the least square x
      // and s that satisfy the linearized constraints, and push it into
      // bounds later.
      SmartPtr<Vector> x_ls = iterates->x()->MakeNew();
      SmartPtr<Vector> s_ls = iterates->s()->MakeNew();
      bool retval =CalculateLeastSquarePrimals(*x_ls, *s_ls);
      if (retval) {
        Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
                       "Least square intial values for x and s computed.\n");
        x_ls->Print(Jnlst(), J_VECTOR, J_INITIALIZATION, "x_ls");
        s_ls->Print(Jnlst(), J_VECTOR, J_INITIALIZATION, "s_ls");
        iterates->Set_x(*x_ls);
        iterates->Set_s(*s_ls);
      }
      else {
        Jnlst().Printf(J_WARNING, J_INITIALIZATION,
                       "Least square initialization of x and s failed!\n");
      }
    }
    DBG_PRINT_VECTOR(2, "curr_x", *iterates->x());

    // Now we compute the initial values that the algorithm is going to
    // actually use.  We first store them in the trial fields in ip_data.

    // Push the x iterates sufficiently inside the bounds
    // Calculate any required shift in x0 and s0

    SmartPtr<const Vector> new_x;
    push_variables(Jnlst(), bound_push_, bound_frac_,
                   "x", *iterates->x(), new_x, *IpNLP().x_L(),
                   *IpNLP().x_U(), *IpNLP().Px_L(), *IpNLP().Px_U());

    iterates->Set_x(*new_x);
    IpData().set_trial(iterates);

    // Calculate the shift in s...
    SmartPtr<const Vector> s = IpCq().trial_d();
    DBG_PRINT_VECTOR(2, "s", *s);

    SmartPtr<const Vector> new_s;
    push_variables(Jnlst(), slack_bound_push_, slack_bound_frac_,
                   "s", *s, new_s, *IpNLP().d_L(),
                   *IpNLP().d_U(), *IpNLP().Pd_L(), *IpNLP().Pd_U());

    iterates = IpData().trial()->MakeNewContainer();
    iterates->Set_s(*new_s);

    /////////////////////////////////////////////////////////////////////
    //                   Initialize bound multipliers                  //
    /////////////////////////////////////////////////////////////////////

    // Initialize the bound multipliers to bound_mult_init_val.
    switch (bound_mult_init_method_) {
    case B_CONSTANT:
      iterates->create_new_z_L();
      iterates->create_new_z_U();
      iterates->create_new_v_L();
      iterates->create_new_v_U();
      iterates->z_L_NonConst()->Set(bound_mult_init_val_);
      iterates->z_U_NonConst()->Set(bound_mult_init_val_);
      iterates->v_L_NonConst()->Set(bound_mult_init_val_);
      iterates->v_U_NonConst()->Set(bound_mult_init_val_);
      IpData().set_trial(iterates);
      break;
    case B_MU_BASED:
      IpData().set_trial(iterates);
      iterates = IpData().trial()->MakeNewContainer();
      iterates->create_new_z_L();
      iterates->create_new_z_U();
      iterates->create_new_v_L();
      iterates->create_new_v_U();
      iterates->z_L_NonConst()->Set(mu_init_);
      iterates->z_U_NonConst()->Set(mu_init_);
      iterates->v_L_NonConst()->Set(mu_init_);
      iterates->v_U_NonConst()->Set(mu_init_);
      iterates->z_L_NonConst()->ElementWiseDivide(*IpCq().trial_slack_x_L());
      iterates->z_U_NonConst()->ElementWiseDivide(*IpCq().trial_slack_x_U());
      iterates->v_L_NonConst()->ElementWiseDivide(*IpCq().trial_slack_s_L());
      iterates->v_U_NonConst()->ElementWiseDivide(*IpCq().trial_slack_s_U());
      IpData().set_trial(iterates);
      break;
    default:
      THROW_EXCEPTION(OPTION_INVALID, "Invalid value of option bound_mult_init_method");
      break;
    }

    bool call_least_square_mults = true;
    if (least_square_init_duals_) {
      // We try to compute a least square estimate of all multiplers,
      // and if successful,we make sure they are sufficiently positive
      SmartPtr<Vector> zL_new = IpData().trial()->z_L()->MakeNew();
      SmartPtr<Vector> zU_new = IpData().trial()->z_U()->MakeNew();
      SmartPtr<Vector> vL_new = IpData().trial()->v_L()->MakeNew();
      SmartPtr<Vector> vU_new = IpData().trial()->v_U()->MakeNew();
      SmartPtr<Vector> yc_new = IpData().trial()->y_c()->MakeNew();
      SmartPtr<Vector> yd_new = IpData().trial()->y_d()->MakeNew();
      bool retval = CalculateLeastSquareDuals(*zL_new, *zU_new, *vL_new,
                                              *vU_new, *yc_new, *yd_new);
      if (retval) {
        // z_L etc are still at bound_mult_init_val_
        zL_new->ElementWiseMax(*IpData().trial()->z_L());
        zU_new->ElementWiseMax(*IpData().trial()->z_U());
        vL_new->ElementWiseMax(*IpData().trial()->v_L());
        vU_new->ElementWiseMax(*IpData().trial()->v_U());
        iterates = IpData().trial()->MakeNewContainer();
        iterates->Set_z_L(*zL_new);
        iterates->Set_z_U(*zU_new);
        iterates->Set_v_L(*vL_new);
        iterates->Set_v_U(*vU_new);
        iterates->Set_y_c(*yc_new);
        iterates->Set_y_d(*yd_new);
        IpData().set_trial(iterates);

        Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
                       "Least square intial values for z_L, z_U,v_L, v_U, y_c, y_d computed.\n");
        zL_new->Print(Jnlst(), J_VECTOR, J_INITIALIZATION, "zL_new");
        zU_new->Print(Jnlst(), J_VECTOR, J_INITIALIZATION, "zU_new");
        vL_new->Print(Jnlst(), J_VECTOR, J_INITIALIZATION, "vL_new");
        vU_new->Print(Jnlst(), J_VECTOR, J_INITIALIZATION, "vU_new");
        yc_new->Print(Jnlst(), J_VECTOR, J_INITIALIZATION, "yc_new");
        yd_new->Print(Jnlst(), J_VECTOR, J_INITIALIZATION, "yd_new");
        call_least_square_mults = false;
      }
      else {
        Jnlst().Printf(J_WARNING, J_INITIALIZATION,
                       "Least square initialization of z_L, z_U,v_L, v_U, y_c, y_d failed!\n");
      }
    }
    if (call_least_square_mults) {
      /////////////////////////////////////////////////////////////////////
      //           Initialize equality constraint multipliers            //
      /////////////////////////////////////////////////////////////////////

      least_square_mults(Jnlst(), IpNLP(), IpData(), IpCq(),
                         eq_mult_calculator_, constr_mult_init_max_);
    }

    // upgrade the trial to the current point
    IpData().AcceptTrialPoint();

    return true;
  }

  bool
  DefaultIterateInitializer::CalculateLeastSquarePrimals(Vector& x_ls, Vector& s_ls)
  {
    DBG_START_METH("DefaultIterateInitializer::CalculateLeastSquarePrimals",
                   dbg_verbosity);
    SmartPtr<const SymMatrix> zeroW = IpNLP().uninitialized_h();
    DBG_PRINT_MATRIX(2, "zeroW", *zeroW);
    SmartPtr<const Matrix> J_c = IpCq().curr_jac_c();
    SmartPtr<const Matrix> J_d = IpCq().curr_jac_d();

    // Compute the right hand side
    SmartPtr<Vector> rhs_x = x_ls.MakeNew();
    rhs_x->Set(0.);
    SmartPtr<Vector> rhs_s = s_ls.MakeNew();
    rhs_s->Set(0.);

    SmartPtr<const Vector> rhs_c = IpCq().curr_c();
    SmartPtr<const Vector> rhs_d = IpCq().curr_d();

    SmartPtr<Vector> sol_c = rhs_c->MakeNew();
    SmartPtr<Vector> sol_d = rhs_d->MakeNew();

    DBG_PRINT_VECTOR(2, "rhs_x", *rhs_x);
    DBG_PRINT_VECTOR(2, "rhs_s", *rhs_s);
    DBG_PRINT_VECTOR(2, "rhs_c", *rhs_c);
    DBG_PRINT_VECTOR(2, "rhs_d", *rhs_d);

    enum ESymSolverStatus retval;
    Index numberOfEVals=rhs_c->Dim()+rhs_d->Dim();
    retval = aug_system_solver_->Solve(GetRawPtr(zeroW), 0.0, NULL, 1.0, NULL,
                                       1.0, GetRawPtr(J_c), NULL, 0.,
                                       GetRawPtr(J_d), NULL, 0., *rhs_x, *rhs_s,
                                       *rhs_c, *rhs_d, x_ls, s_ls, *sol_c, *sol_d,
                                       true, numberOfEVals);
    if (retval!=SYMSOLVER_SUCCESS) {
      return false;
    }
    x_ls.Scal(-1.);
    s_ls.Scal(-1.);

    DBG_PRINT_VECTOR(2, "sol_x", x_ls);
    DBG_PRINT_VECTOR(2, "sol_s", s_ls);
    DBG_PRINT_VECTOR(2, "sol_c", *sol_c);
    DBG_PRINT_VECTOR(2, "sol_d", *sol_d);

    return true;
  }

  bool
  DefaultIterateInitializer::
  CalculateLeastSquareDuals(Vector& zL_new, Vector& zU_new,
                            Vector& vL_new, Vector& vU_new,
                            Vector& yc_new, Vector& yd_new)
  {
    DBG_START_METH("DefaultIterateInitializer::CalculateLeastSquarePrimals",
                   dbg_verbosity);

    SmartPtr<const SymMatrix> zeroW = IpNLP().uninitialized_h();
    DBG_PRINT_MATRIX(2, "zeroW", *zeroW);
    SmartPtr<const Matrix> J_c = IpCq().curr_jac_c();
    SmartPtr<const Matrix> J_d = IpCq().curr_jac_d();

    // Compute the entries in Hessian diagonals
    SmartPtr<Vector> Dx = IpData().trial()->x()->MakeNew();
    SmartPtr<Vector> tmp = IpNLP().x_L()->MakeNew();
    tmp->Set(-1.);
    IpNLP().Px_L()->MultVector(1., *tmp, 0., *Dx);
    tmp = IpNLP().x_U()->MakeNew();
    tmp->Set(-1.);
    IpNLP().Px_U()->MultVector(1., *tmp, 1., *Dx);
    SmartPtr<Vector> Ds = IpData().trial()->s()->MakeNew();
    tmp = IpNLP().d_L()->MakeNew();
    tmp->Set(-1.);
    IpNLP().Pd_L()->MultVector(1., *tmp, 0., *Ds);
    tmp = IpNLP().d_U()->MakeNew();
    tmp->Set(-1.);
    IpNLP().Pd_U()->MultVector(1., *tmp, 1., *Ds);

    // Get the right hand side
    SmartPtr<const Vector> rhs_x = IpCq().trial_grad_f();
    SmartPtr<Vector> rhs_s = Ds->MakeNew();
    rhs_s->Set(0.);
    SmartPtr<Vector> rhs_c = yc_new.MakeNew();
    rhs_c->Set(0.);
    SmartPtr<Vector> rhs_d = yd_new.MakeNew();
    rhs_d->Set(0.);

    // Space for the solution
    SmartPtr<Vector> sol_x = rhs_x->MakeNew();
    SmartPtr<Vector> sol_s = rhs_s->MakeNew();

    DBG_PRINT_VECTOR(2, "rhs_x", *rhs_x);
    DBG_PRINT_VECTOR(2, "rhs_s", *rhs_s);
    DBG_PRINT_VECTOR(2, "rhs_c", *rhs_c);
    DBG_PRINT_VECTOR(2, "rhs_d", *rhs_d);

    enum ESymSolverStatus retval;
    Index numberOfEVals=rhs_x->Dim()+rhs_s->Dim();
    retval =
      aug_system_solver_->Solve(GetRawPtr(zeroW), 0.0, GetRawPtr(Dx), 0.0, GetRawPtr(Ds),
                                0.0, GetRawPtr(J_c), NULL, 0.,
                                GetRawPtr(J_d), NULL, 0., *rhs_x, *rhs_s,
                                *rhs_c, *rhs_d, *sol_x, *sol_s,
                                yc_new, yd_new, true, numberOfEVals);
    if (retval!=SYMSOLVER_SUCCESS) {
      return false;
    }
    DBG_PRINT_VECTOR(2, "sol_x", *sol_x);
    DBG_PRINT_VECTOR(2, "sol_s", *sol_s);
    DBG_PRINT_VECTOR(2, "sol_c", yc_new);
    DBG_PRINT_VECTOR(2, "sol_d", yd_new);

    // Get the output right
    yc_new.Scal(-1.0);
    yd_new.Scal(-1.0);
    IpNLP().Px_L()->TransMultVector(-1., *sol_x, 0., zL_new);
    IpNLP().Px_U()->TransMultVector(1., *sol_x, 0., zU_new);
    IpNLP().Pd_L()->TransMultVector(-1., *sol_s, 0., vL_new);
    IpNLP().Pd_U()->TransMultVector(1., *sol_s, 0., vU_new);

    return true;
  }

  void DefaultIterateInitializer::push_variables(
    const Journalist& jnlst,
    Number bound_push,
    Number bound_frac,
    std::string name,
    const Vector& orig_x,
    SmartPtr<const Vector>& new_x,
    const Vector& x_L,
    const Vector& x_U,
    const Matrix& Px_L,
    const Matrix& Px_U)
  {
    DBG_START_FUN("DefaultIterateInitializer::push_variables",
                  dbg_verbosity);

    SmartPtr<const Vector> my_orig_x = &orig_x;
    // ToDo: Make this more efficient...?

    // To avoid round-off error, move variables first at the bounds
    if (bound_push>0. || bound_frac>0.) {
      push_variables(jnlst, 0., 0., name, orig_x, new_x, x_L, x_U, Px_L, Px_U);
      my_orig_x = new_x;
    }

    DBG_PRINT_VECTOR(2,"orig_x", *my_orig_x);
    DBG_PRINT_MATRIX(2,"Px_L", Px_L);
    DBG_PRINT_VECTOR(2, "x_L", x_L);
    DBG_PRINT_MATRIX(2,"Px_U", Px_U);
    DBG_PRINT_VECTOR(2, "x_U", x_U);

    SmartPtr<Vector> tmp_l = x_L.MakeNew();
    SmartPtr<Vector> tmp_u = x_U.MakeNew();

    const double dbl_min = std::numeric_limits<double>::min();
    const double tiny_double = 100.0*dbl_min;

    // Calculate any required shift in x0 and s0
    SmartPtr<Vector> tmp = my_orig_x->MakeNew();
    SmartPtr<Vector> tiny_l = x_L.MakeNew();
    tiny_l->Set(tiny_double);

    SmartPtr<Vector> q_l = x_L.MakeNew();
    SmartPtr<Vector> p_l = x_L.MakeNew();
    SmartPtr<Vector> delta_x = my_orig_x->MakeNew();

    SmartPtr<Vector> zero_l = x_L.MakeNew();
    zero_l->Set(0.0);
    SmartPtr<Vector> zero_u = x_U.MakeNew();
    zero_u->Set(0.0);

    if (bound_frac>0.) {
      DBG_ASSERT(bound_push>0.);

      // Calculate p_l
      Px_L.MultVector(1.0, x_L, 0.0, *tmp);
      Px_U.TransMultVector(1.0, *tmp, 0.0, *tmp_u);
      tmp_u->AddOneVector(1., x_U, -1.);
      Px_U.MultVector(1.0, *tmp_u, 0.0, *tmp);
      Px_L.TransMultVector(1.0, *tmp, 0.0, *q_l);
      q_l->AddOneVector(-1.0, *tiny_l, bound_frac);
      DBG_PRINT_VECTOR(2, "q_l", *q_l);
      // At this point, q_l is
      // bound_frac * Px_L^T Px_U(x_U - Px_U^T Px_L x_L)  -  tiny_double
      // i.e., it is bound_frac*(x_U - x_L) for those components that have
      //          two bounds
      //       and - tiny_double for those that have only one or no bounds

      tmp_l->Set(bound_push);
      p_l->AddOneVector(bound_push, x_L, 0.);
      p_l->ElementWiseAbs();
      p_l->ElementWiseMax(*tmp_l);
      // now p_l is bound_push * max(|x_L|,1)

      q_l->ElementWiseReciprocal();
      p_l->ElementWiseReciprocal();

      p_l->ElementWiseMax(*q_l);
      p_l->ElementWiseReciprocal();
      //    p_l->Axpy(1.0, *tiny_l);  we shouldn't need this

      // At this point, p_l is
      //  min(bound_push * max(|x_L|,1), bound_frac*(x_U-x_L)  for components
      //                                                       with two bounds
      //  bound_push * max(|x_L|,1)                            otherwise
      // This is the margin we want to the lower bound
      DBG_PRINT_VECTOR(1, "p_l", *p_l);

      // Calculate p_u
      SmartPtr<Vector> q_u = x_U.MakeNew();
      SmartPtr<Vector> p_u = x_U.MakeNew();
      SmartPtr<Vector> tiny_u = x_U.MakeNew();
      tiny_u->Set(tiny_double);

      Px_U.MultVector(1.0, x_U, 0.0, *tmp);
      Px_L.TransMultVector(1.0, *tmp, 0.0, *tmp_l);
      tmp_l->Axpy(-1.0, x_L);
      Px_L.MultVector(1.0, *tmp_l, 0.0, *tmp);
      Px_U.TransMultVector(1.0, *tmp, 0.0, *q_u);
      q_u->AddOneVector(-1.0, *tiny_u, bound_frac);
      DBG_PRINT_VECTOR(2,"q_u",*q_u);
      // q_u is now the same as q_l above, but of the same dimension as x_L

      tmp_u->Set(bound_push);
      p_u->Copy(x_U);
      p_u->AddOneVector(bound_push, x_U, 0.);
      p_u->ElementWiseAbs();
      p_u->ElementWiseMax(*tmp_u);
      DBG_PRINT_VECTOR(2,"p_u",*p_u);

      q_u->ElementWiseReciprocal();
      p_u->ElementWiseReciprocal();

      p_u->ElementWiseMax(*q_u);
      p_u->ElementWiseReciprocal();
      p_u->Axpy(1.0, *tiny_u);
      // At this point, p_l is
      //  min(bound_push * max(|x_U|,1), bound_frac*(x_U-x_L)  for components
      //                                                       with two bounds
      //  bound_push * max(|x_U|,1)                            otherwise
      // This is the margin we want to the upper bound
      DBG_PRINT_VECTOR(2,"actual_p_u",*p_u);

      // Calculate the new x

      Px_L.TransMultVector(-1.0, *my_orig_x, 0.0, *tmp_l);
      DBG_PRINT_VECTOR(1, "tmp_l1", *tmp_l);
      tmp_l->AddTwoVectors(1.0, x_L, 1.0, *p_l, 1.);
      tmp_l->ElementWiseMax(*zero_l);
      DBG_PRINT_VECTOR(1, "tmp_l2", *tmp_l);
      // tmp_l is now max(x_L + p_l - x, 0), i.e., the amount by how
      // much need to correct the variable

      Px_U.TransMultVector(1.0, *my_orig_x, 0.0, *tmp_u);
      tmp_u->AddTwoVectors(-1.0, x_U, 1.0, *p_u, 1.);
      tmp_u->ElementWiseMax(*zero_u);
      // tmp_u is now max(x - (x_U-p_u), 0), i.e., the amount by how
      // much need to correct the variable
    }
    else {
      DBG_ASSERT(bound_push == 0.);
      tmp_l = x_L.MakeNewCopy();
      Px_L.TransMultVector(-1.0, *my_orig_x, 1.0, *tmp_l);
      tmp_l->ElementWiseMax(*zero_l);
      DBG_PRINT_VECTOR(1, "tmp_l3", *tmp_l);

      tmp_u = x_U.MakeNewCopy();
      Px_U.TransMultVector(1.0, *my_orig_x, -1.0, *tmp_u);
      tmp_u->ElementWiseMax(*zero_u);
    }

    Number nrm_l = tmp_l->Amax();
    if (nrm_l>0.) {
      Px_L.MultVector(1.0, *tmp_l, 0.0, *delta_x);
    }
    else {
      delta_x->Set(0.);
    }
    Number nrm_u = tmp_u->Amax();
    if (nrm_u>0.) {
      Px_U.MultVector(-1.0, *tmp_u, 1.0, *delta_x);
    }

    if (nrm_l > 0 || nrm_u > 0) {
      delta_x->Axpy(1.0, *my_orig_x);
      new_x = ConstPtr(delta_x);
      if (bound_push > 0.) {
        jnlst.Printf(J_DETAILED, J_INITIALIZATION,
                     "Moved initial values of %s sufficiently inside the bounds.\n", name.c_str());
        my_orig_x->Print(jnlst, J_VECTOR, J_INITIALIZATION, "original vars");
        new_x->Print(jnlst, J_VECTOR, J_INITIALIZATION, "new vars");
      }
    }
    else {
      new_x = my_orig_x;
      if (bound_push > 0.) {
        jnlst.Printf(J_DETAILED, J_INITIALIZATION,
                     "Initial values of %s sufficiently inside the bounds.\n", name.c_str());
      }
    }
  }

  void DefaultIterateInitializer::least_square_mults(
    const Journalist& jnlst,
    IpoptNLP& ip_nlp,
    IpoptData& ip_data,
    IpoptCalculatedQuantities& ip_cq,
    const SmartPtr<EqMultiplierCalculator>& eq_mult_calculator,
    Number constr_mult_init_max)
  {
    DBG_START_FUN("DefaultIterateInitializer::least_square_mults",
                  dbg_verbosity);

    SmartPtr<IteratesVector> iterates = ip_data.trial()->MakeNewContainer();
    iterates->create_new_y_c();
    iterates->create_new_y_d();

    if (iterates->y_c_NonConst()->Dim()==iterates->x()->Dim()) {
      // This problem is square, we just set the multipliers to zero
      iterates->y_c_NonConst()->Set(0.0);
      iterates->y_d_NonConst()->Set(0.0);
      ip_data.Append_info_string("s");
    }
    else if (IsValid(eq_mult_calculator) && constr_mult_init_max>0. &&
             iterates->y_c_NonConst()->Dim()+iterates->y_d_NonConst()->Dim()>0) {
      // First move all the trial data into the current fields, since
      // those values are needed to compute the initial values for
      // the multipliers
      ip_data.CopyTrialToCurrent();
      SmartPtr<Vector> y_c = iterates->y_c_NonConst();
      SmartPtr<Vector> y_d = iterates->y_d_NonConst();
      bool retval = eq_mult_calculator->CalculateMultipliers(*y_c, *y_d);
      if (!retval) {
        y_c->Set(0.0);
        y_d->Set(0.0);
      }
      else {
        /*
        {
          ip_data.set_trial(iterates);
          printf("grad_x = %e grad_s = %e y_c = %e y_d = %e\n",
          ip_cq.trial_grad_lag_x()->Amax(),
          ip_cq.trial_grad_lag_s()->Amax(),
          y_c->Amax(),
          y_d->Amax());
          iterates = ip_data.trial()->MakeNewContainer();
        }
        */
        jnlst.Printf(J_DETAILED, J_INITIALIZATION,
                     "Least square estimates max(y_c) = %e, max(y_d) = %e\n",
                     y_c->Amax(), y_d->Amax());
        Number yinitnrm = Max(y_c->Amax(), y_d->Amax());
        if (yinitnrm > constr_mult_init_max) {
          y_c->Set(0.0);
          y_d->Set(0.0);
        }
        else {
          ip_data.Append_info_string("y");
        }
      }
    }
    else {
      iterates->y_c_NonConst()->Set(0.0);
      iterates->y_d_NonConst()->Set(0.0);
    }
    ip_data.set_trial(iterates);

    DBG_PRINT_VECTOR(2, "y_c", *ip_data.trial()->y_c());
    DBG_PRINT_VECTOR(2, "y_d", *ip_data.trial()->y_d());
  }

} // namespace Ipopt
