// Copyright (C) 2004, 2007 International Business Machines and others.
// All Rights Reserved.
// This code is published under the Common Public License.
//
// $Id: IpGradientScaling.cpp 1101 2007-09-29 00:07:17Z andreasw $
//
// Authors:  Carl Laird, Andreas Waechter     IBM    2004-07-13

#include "IpGradientScaling.hpp"
#include "IpTripletHelper.hpp"
#include "IpBlas.hpp"

#ifdef HAVE_CMATH
# include <cmath>
#else
# ifdef HAVE_MATH_H
#  include <math.h>
# else
#  error "don't have header file for math"
# endif
#endif

namespace Ipopt
{

  void GradientScaling::RegisterOptions(const SmartPtr<RegisteredOptions>& roptions)
  {
    roptions->AddLowerBoundedNumberOption(
      "nlp_scaling_max_gradient", "Maximum gradient after NLP scaling.",
      0, true, 100.0,
      "This is the gradient scaling cut-off. If the maximum"
      " gradient is above this value, then gradient based scaling"
      " will be performed. Scaling parameters are calculated to"
      " scale the maximum gradient back to this value. (This is g_max in "
      "Section 3.8 of the implementation paper.) Note: This"
      " option is only used if \"nlp_scaling_method\" is chosen as"
      " \"gradient-based\".");
    roptions->AddLowerBoundedNumberOption(
      "nlp_scaling_obj_target_gradient",
      "Target value for objective function gradient size.",
      0, false, 0.,
      "If a positive number is chosen, the scaling factor the objective "
      "function is computed so that the gradient has the max norm of the given "
      "size at the starting point.  This overrides nlp_scaling_max_gradient "
      "for the objective function.");
    roptions->AddLowerBoundedNumberOption(
      "nlp_scaling_constr_target_gradient",
      "Target value for constraint function gradient size.",
      0, false, 0.,
      "If a positive number is chosen, the scaling factor the constraint "
      "functions is computed so that the gradient has the max norm of the given "
      "size at the starting point.  This overrides nlp_scaling_max_gradient "
      "for the constraint functions.");
  }

  bool GradientScaling::InitializeImpl(const OptionsList& options,
                                       const std::string& prefix)
  {
    options.GetNumericValue("nlp_scaling_max_gradient",
                            scaling_max_gradient_, prefix);
    options.GetNumericValue("nlp_scaling_obj_target_gradient",
                            scaling_obj_target_gradient_, prefix);
    options.GetNumericValue("nlp_scaling_constr_target_gradient",
                            scaling_constr_target_gradient_, prefix);
    return StandardScalingBase::InitializeImpl(options, prefix);
  }


  void GradientScaling::DetermineScalingParametersImpl(
    const SmartPtr<const VectorSpace> x_space,
    const SmartPtr<const VectorSpace> c_space,
    const SmartPtr<const VectorSpace> d_space,
    const SmartPtr<const MatrixSpace> jac_c_space,
    const SmartPtr<const MatrixSpace> jac_d_space,
    const SmartPtr<const SymMatrixSpace> h_space,
    const Matrix& Px_L, const Vector& x_L,
    const Matrix& Px_U, const Vector& x_U,
    Number& df,
    SmartPtr<Vector>& dx,
    SmartPtr<Vector>& dc,
    SmartPtr<Vector>& dd)
  {
    DBG_ASSERT(IsValid(nlp_));

    SmartPtr<Vector> x = x_space->MakeNew();
    if (!nlp_->GetStartingPoint(GetRawPtr(x), true,
                                NULL, false,
                                NULL, false,
                                NULL, false,
                                NULL, false)) {
      THROW_EXCEPTION(FAILED_INITIALIZATION,
                      "Error getting initial point from NLP in GradientScaling.\n");
    }

    //
    // Calculate grad_f scaling
    //
    SmartPtr<Vector> grad_f = x_space->MakeNew();
    if (nlp_->Eval_grad_f(*x, *grad_f)) {
      double max_grad_f = grad_f->Amax();
      df = 1.;
      if (scaling_obj_target_gradient_ == 0.) {
        if (max_grad_f > scaling_max_gradient_) {
          df = scaling_max_gradient_ / max_grad_f;
        }
      }
      else {
        if (max_grad_f == 0.) {
          Jnlst().Printf(J_WARNING, J_INITIALIZATION,
                         "Gradient of objective function is zero at starting point.  Cannot determine scaling factor based on scaling_obj_target_gradient option.\n");
        }
        else {
          df = scaling_obj_target_gradient_ / max_grad_f;
        }
      }
      Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
                     "Scaling parameter for objective function = %e\n", df);
    }
    else {
      Jnlst().Printf(J_WARNING, J_INITIALIZATION,
                     "Error evaluating objective gradient at user provided starting point.\n  No scaling factor for objective function computed!\n");
      df = 1.;
    }
    //
    // No x scaling
    //
    dx = NULL;

    dc = NULL;
    if (c_space->Dim()>0) {
      //
      // Calculate c scaling
      //
      SmartPtr<Matrix> jac_c = jac_c_space->MakeNew();
      if (nlp_->Eval_jac_c(*x, *jac_c)) {
        // ToDo: Don't use TripletHelper, have special methods on matrices instead
        Index nnz = TripletHelper::GetNumberEntries(*jac_c);
        Index* irow = new Index[nnz];
        Index* jcol = new Index[nnz];
        Number* values = new Number[nnz];
        TripletHelper::FillRowCol(nnz, *jac_c, irow, jcol);
        TripletHelper::FillValues(nnz, *jac_c, values);

        if (scaling_constr_target_gradient_<=0.) {
          Number* c_scaling = new Number[jac_c->NRows()];

          for (Index r=0; r<jac_c->NRows(); r++) {
            c_scaling[r] = 0;
          }

          // put the max of each row into c_scaling...
          bool need_c_scale = false;
          for (Index i=0; i<nnz; i++) {
            if (fabs(values[i]) > scaling_max_gradient_) {
              c_scaling[irow[i]-1] = Max(c_scaling[irow[i]-1], fabs(values[i]));
              need_c_scale = true;
            }
          }

          if (need_c_scale) {
            // now compute the scaling factors for each row
            for (Index r=0; r<jac_c->NRows(); r++) {
              Number scaling = 1.0;
              if (c_scaling[r] > scaling_max_gradient_) {
                scaling = scaling_max_gradient_/c_scaling[r];
              }
              c_scaling[r] = scaling;
            }

            dc = c_space->MakeNew();
            TripletHelper::PutValuesInVector(jac_c->NRows(), c_scaling, *dc);
            if (Jnlst().ProduceOutput(J_DETAILED, J_INITIALIZATION)) {
              Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
                             "Equality constraints are scaled with smallest scaling parameter is %e\n", dc->Min());
            }
          }
          else {
            Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
                           "Equality constraints are not scaled.\n");
            dc = NULL;
          }
          delete [] c_scaling;
        }
        else {
          // Compute the largest element in the Jacobian
          Index i = IpBlasIdamax(nnz, values, 1)-1;
          Number c_scaling = scaling_constr_target_gradient_/fabs(values[i]);
          dc = c_space->MakeNew();
          dc->Set(c_scaling);
          if (Jnlst().ProduceOutput(J_DETAILED, J_INITIALIZATION)) {
            Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
                           "Equality constraints are scaled uniformly by %e\n", c_scaling);
          }
        }

        delete [] irow;
        irow = NULL;
        delete [] jcol;
        jcol = NULL;
        delete [] values;
        values = NULL;
      }
      else {
        Jnlst().Printf(J_WARNING, J_INITIALIZATION,
                       "Error evaluating Jacobian of equality constraints at user provided starting point.\n  No scaling factors for equality constraints computed!\n");
      }
    }

    dd = NULL;
    if (d_space->Dim()>0) {
      //
      // Calculate d scaling
      //
      SmartPtr<Matrix> jac_d = jac_d_space->MakeNew();
      if (nlp_->Eval_jac_d(*x, *jac_d)) {
        Index nnz = TripletHelper::GetNumberEntries(*jac_d);
        Index* irow = new Index[nnz];
        Index* jcol = new Index[nnz];
        Number* values = new Number[nnz];
        TripletHelper::FillRowCol(nnz, *jac_d, irow, jcol);
        TripletHelper::FillValues(nnz, *jac_d, values);

        if (scaling_constr_target_gradient_<=0.) {
          Number* d_scaling = new Number[jac_d->NRows()];

          for (Index r=0; r<jac_d->NRows(); r++) {
            d_scaling[r] = 0;
          }

          // put the max of each row into c_scaling...
          bool need_d_scale = false;
          for (Index i=0; i<nnz; i++) {
            if (fabs(values[i]) > scaling_max_gradient_) {
              d_scaling[irow[i]-1] = Max(d_scaling[irow[i]-1], fabs(values[i]));
              need_d_scale = true;
            }
          }

          if (need_d_scale) {
            // now compute the scaling factors for each row
            for (Index r=0; r<jac_d->NRows(); r++) {
              Number scaling = 1.0;
              if (d_scaling[r] > scaling_max_gradient_) {
                scaling = scaling_max_gradient_/d_scaling[r];
              }
              d_scaling[r] = scaling;
            }

            dd = d_space->MakeNew();
            TripletHelper::PutValuesInVector(jac_d->NRows(), d_scaling, *dd);
            if (Jnlst().ProduceOutput(J_DETAILED, J_INITIALIZATION)) {
              Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
                             "Inequality constraints are scaled with smallest scaling parameter is %e\n", dd->Min());
            }
          }
          else {
            dd = NULL;
            Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
                           "Inequality constraints are not scaled.\n");
          }
          delete [] d_scaling;
        }
        else {
          // Compute the largest element in the Jacobian
          Index i = IpBlasIdamax(nnz, values, 1)-1;
          Number d_scaling = scaling_constr_target_gradient_/fabs(values[i]);
          dd = d_space->MakeNew();
          dd->Set(d_scaling);
          if (Jnlst().ProduceOutput(J_DETAILED, J_INITIALIZATION)) {
            Jnlst().Printf(J_DETAILED, J_INITIALIZATION,
                           "Inequality constraints are scaled uniformly by %e\n", d_scaling);
          }
        }

        delete [] irow;
        irow = NULL;
        delete [] jcol;
        jcol = NULL;
        delete [] values;
        values = NULL;
      }
      else {
        Jnlst().Printf(J_WARNING, J_INITIALIZATION,
                       "Error evaluating Jacobian of inequality constraints at user provided starting point.\n  No scaling factors for inequality constraints computed!\n");
      }
    }
  }

} // namespace Ipopt
