/* $Id$ */
// Copyright (C) 2003, International Business Machines
// Corporation and others.  All Rights Reserved.
// This code is licensed under the terms of the Eclipse Public License (EPL).

#include "ClpLsqr.hpp"
#include "ClpPdco.hpp"

void ClpLsqr::do_lsqr(CoinDenseVector< double > &b,
  double damp, double atol, double btol, double conlim, int itnlim,
  bool show, Info info, CoinDenseVector< double > &x, int *istop,
  int *itn, Outfo *outfo, bool precon, CoinDenseVector< double > &Pr)
{

  /**
     Special version of LSQR for use with pdco.m.
     It continues with a reduced atol if a pdco-specific test isn't
     satisfied with the input atol.
     */

  //     Initialize.
  static char term_msg[8][80] = {
    "The exact solution is x = 0",
    "The residual Ax - b is small enough, given ATOL and BTOL",
    "The least squares error is small enough, given ATOL",
    "The estimated condition number has exceeded CONLIM",
    "The residual Ax - b is small enough, given machine precision",
    "The least squares error is small enough, given machine precision",
    "The estimated condition number has exceeded machine precision",
    "The iteration limit has been reached"
  };

  //  printf("***************** Entering LSQR *************\n");
  assert(model_);

  char str1[100], str2[100], str3[100], str4[100], head1[100], head2[100];

  int n = ncols_; // set  m,n from lsqr object

  *itn = 0;
  *istop = 0;
  double ctol = 0;
  if (conlim > 0)
    ctol = 1 / conlim;

  double anorm = 0;
  double acond = 0;
  double ddnorm = 0;
  double xnorm = 0;
  double xxnorm = 0;
  double z = 0;
  double cs2 = -1;
  double sn2 = 0;

  // Set up the first vectors u and v for the bidiagonalization.
  // These satisfy  beta*u = b,  alfa*v = A'u.

  CoinDenseVector< double > u(b);
  CoinDenseVector< double > v(n, 0.0);
  x.clear();
  double alfa = 0;
  double beta = u.twoNorm();
  if (beta > 0) {
    u = (1 / beta) * u;
    matVecMult(2, v, u);
    if (precon)
      v = v * Pr;
    alfa = v.twoNorm();
  }
  if (alfa > 0) {
    v.scale(1 / alfa);
  }
  CoinDenseVector< double > w(v);

  double arnorm = alfa * beta;
  if (arnorm == 0) {
    printf("  %s\n\n", term_msg[0]);
    return;
  }

  double rhobar = alfa;
  double phibar = beta;
  double bnorm = beta;
  double rnorm = beta;
  sprintf(head1, "   Itn      x(1)      Function");
  sprintf(head2, " Compatible   LS      Norm A   Cond A");

  if (show) {
    printf(" %s%s\n", head1, head2);
    double test1 = 1;
    double test2 = alfa / beta;
    sprintf(str1, "%6d %12.5e %10.3e", *itn, x[0], rnorm);
    sprintf(str2, "  %8.1e  %8.1e", test1, test2);
    printf("%s%s\n", str1, str2);
  }

  //----------------------------------------------------------------
  // Main iteration loop.
  //----------------------------------------------------------------
  while (*itn < itnlim) {
    *itn += 1;
    // Perform the next step of the bidiagonalization to obtain the
    // next beta, u, alfa, v.  These satisfy the relations
    // beta*u  =  a*v   -  alfa*u,
    // alfa*v  =  A'*u  -  beta*v.

    u.scale((-alfa));
    if (precon) {
      CoinDenseVector< double > pv(v * Pr);
      matVecMult(1, u, pv);
    } else {
      matVecMult(1, u, v);
    }
    beta = u.twoNorm();
    if (beta > 0) {
      u.scale((1 / beta));
      anorm = sqrt(anorm * anorm + alfa * alfa + beta * beta + damp * damp);
      v.scale((-beta));
      CoinDenseVector< double > vv(n);
      vv.clear();
      matVecMult(2, vv, u);
      if (precon)
        vv = vv * Pr;
      v = v + vv;
      alfa = v.twoNorm();
      if (alfa > 0)
        v.scale((1 / alfa));
    }

    // Use a plane rotation to eliminate the damping parameter.
    // This alters the diagonal (rhobar) of the lower-bidiagonal matrix.

    double rhobar1 = sqrt(rhobar * rhobar + damp * damp);
    double cs1 = rhobar / rhobar1;
    double sn1 = damp / rhobar1;
    double psi = sn1 * phibar;
    phibar *= cs1;

    // Use a plane rotation to eliminate the subdiagonal element (beta)
    // of the lower-bidiagonal matrix, giving an upper-bidiagonal matrix.

    double rho = sqrt(rhobar1 * rhobar1 + beta * beta);
    double cs = rhobar1 / rho;
    double sn = beta / rho;
    double theta = sn * alfa;
    rhobar = -cs * alfa;
    double phi = cs * phibar;
    phibar = sn * phibar;
    double tau = sn * phi;

    // Update x and w.

    double t1 = phi / rho;
    double t2 = -theta / rho;
    //    dk           =   ((1/rho)*w);

    double w_norm = w.twoNorm();
    x = x + t1 * w;
    w = v + t2 * w;
    ddnorm = ddnorm + (w_norm / rho) * (w_norm / rho);
    // if wantvar, var = var  +  dk.*dk; end

    // Use a plane rotation on the right to eliminate the
    // super-diagonal element (theta) of the upper-bidiagonal matrix.
    // Then use the result to estimate  norm(x).

    double delta = sn2 * rho;
    double gambar = -cs2 * rho;
    double rhs = phi - delta * z;
    double zbar = rhs / gambar;
    xnorm = sqrt(xxnorm + zbar * zbar);
    double gamma = sqrt(gambar * gambar + theta * theta);
    cs2 = gambar / gamma;
    sn2 = theta / gamma;
    z = rhs / gamma;
    xxnorm = xxnorm + z * z;

    // Test for convergence.
    // First, estimate the condition of the matrix  Abar,
    // and the norms of  rbar  and  Abar'rbar.

    acond = anorm * sqrt(ddnorm);
    double res1 = phibar * phibar;
    double res2 = res1 + psi * psi;
    rnorm = sqrt(res1 + res2);
    arnorm = alfa * fabs(tau);

    // Now use these norms to estimate certain other quantities,
    // some of which will be small near a solution.

    double test1 = rnorm / bnorm;
    double test2 = arnorm / (anorm * rnorm);
    double test3 = 1 / acond;
    t1 = test1 / (1 + anorm * xnorm / bnorm);
    double rtol = btol + atol * anorm * xnorm / bnorm;

    // The following tests guard against extremely small values of
    // atol, btol  or  ctol.  (The user may have set any or all of
    // the parameters  atol, btol, conlim  to 0.)
    // The effect is equivalent to the normal tests using
    // atol = eps,  btol = eps,  conlim = 1/eps.

    if (*itn >= itnlim)
      *istop = 7;
    if (1 + test3 <= 1)
      *istop = 6;
    if (1 + test2 <= 1)
      *istop = 5;
    if (1 + t1 <= 1)
      *istop = 4;

    // Allow for tolerances set by the user.

    if (test3 <= ctol)
      *istop = 3;
    if (test2 <= atol)
      *istop = 2;
    if (test1 <= rtol)
      *istop = 1;

    //-------------------------------------------------------------------
    // SPECIAL TEST THAT DEPENDS ON pdco.m.
    // Aname in pdco   is  iw in lsqr.
    // dy              is  x
    // Other stuff     is in info.

    // We allow for diagonal preconditioning in pdDDD3.
    //-------------------------------------------------------------------
    if (*istop > 0) {
      double r3new = arnorm;
      double r3ratio = r3new / info.r3norm;
      double atolold = atol;
      double atolnew = atol;

      if (atol > info.atolmin) {
        if (r3ratio <= 0.1) { // dy seems good
          // Relax
        } else if (r3ratio <= 0.5) { // Accept dy but make next one more accurate.
          atolnew = atolnew * 0.1;
        } else { // Recompute dy more accurately
          if (show) {
            printf("\n                                ");
            printf("                                \n");
            printf(" %5.1f%7d%7.3f", log10(atolold), *itn, r3ratio);
          }
          atol = atol * 0.1;
          atolnew = atol;
          *istop = 0;
        }

        outfo->atolold = atolold;
        outfo->atolnew = atolnew;
        outfo->r3ratio = r3ratio;
      }

      //-------------------------------------------------------------------
      // See if it is time to print something.
      //-------------------------------------------------------------------
      int prnt = 0;
      if (n <= 40)
        prnt = 1;
      if (*itn <= 10)
        prnt = 1;
      if (*itn >= itnlim - 10)
        prnt = 1;
      if (*itn % 10 == 0)
        prnt = 1;
      if (test3 <= 2 * ctol)
        prnt = 1;
      if (test2 <= 10 * atol)
        prnt = 1;
      if (test1 <= 10 * rtol)
        prnt = 1;
      if (*istop != 0)
        prnt = 1;

      if (prnt == 1) {
        if (show) {
          sprintf(str1, "   %6d %12.5e %10.3e", *itn, x[0], rnorm);
          sprintf(str2, "  %8.1e %8.1e", test1, test2);
          sprintf(str3, " %8.1e %8.1e", anorm, acond);
          printf("%s%s%s\n", str1, str2, str3);
        }
      }
      if (*istop > 0)
        break;
    }
  }
  // End of iteration loop.
  // Print the stopping condition.

  if (show) {
    printf("\n LSQR finished\n");
    //    disp(msg(istop+1,:))
    //    disp(' ')
    printf("%s\n", term_msg[*istop]);
    sprintf(str1, "istop  =%8d     itn    =%8d", *istop, *itn);
    sprintf(str2, "anorm  =%8.1e   acond  =%8.1e", anorm, acond);
    sprintf(str3, "rnorm  =%8.1e   arnorm =%8.1e", rnorm, arnorm);
    sprintf(str4, "bnorm  =%8.1e   xnorm  =%8.1e", bnorm, xnorm);
    printf("%s %s\n", str1, str2);
    printf("%s %s\n", str3, str4);
  }
}

void ClpLsqr::matVecMult(int mode, CoinDenseVector< double > *x, CoinDenseVector< double > *y)
{
  int n = model_->numberColumns();
  int m = model_->numberRows();
  CoinDenseVector< double > *temp = new CoinDenseVector< double >(n, 0.0);
  double *t_elts = temp->getElements();
  double *x_elts = x->getElements();
  double *y_elts = y->getElements();
  ClpPdco *pdcoModel = (ClpPdco *)model_;
  if (mode == 1) {
    pdcoModel->matVecMult(2, temp, y);
    for (int k = 0; k < n; k++)
      x_elts[k] += (diag1_[k] * t_elts[k]);
    for (int k = 0; k < m; k++)
      x_elts[n + k] += (diag2_ * y_elts[k]);
  } else {
    for (int k = 0; k < n; k++)
      t_elts[k] = diag1_[k] * y_elts[k];
    pdcoModel->matVecMult(1, x, temp);
    for (int k = 0; k < m; k++)
      x_elts[k] += diag2_ * y_elts[n + k];
  }
  delete temp;
  return;
}

void ClpLsqr::matVecMult(int mode, CoinDenseVector< double > &x, CoinDenseVector< double > &y)
{
  matVecMult(mode, &x, &y);
  return;
}
/* Default constructor */
ClpLsqr::ClpLsqr()
  : nrows_(0)
  , ncols_(0)
  , model_(NULL)
  , diag1_(NULL)
  , diag2_(0.0)
{
}

/* Constructor for use with Pdco model (note modified for pdco!!!!) */
ClpLsqr::ClpLsqr(ClpInterior *model)
  : diag1_(NULL)
  , diag2_(0.0)
{
  model_ = model;
  nrows_ = model->numberRows() + model->numberColumns();
  ncols_ = model->numberRows();
}
/** Destructor */
ClpLsqr::~ClpLsqr()
{
  // delete [] diag1_; no as we just borrowed it
}
bool ClpLsqr::setParam(char *parmName, int parmValue)
{
  std::cout << "Set lsqr integer parameter " << parmName << "to " << parmValue
            << std::endl;
  if (strcmp(parmName, "nrows") == 0) {
    nrows_ = parmValue;
    return 1;
  } else if (strcmp(parmName, "ncols") == 0) {
    ncols_ = parmValue;
    return 1;
  }
  std::cout << "Attempt to set unknown integer parameter name " << parmName << std::endl;
  return 0;
}
ClpLsqr::ClpLsqr(const ClpLsqr &rhs)
  : nrows_(rhs.nrows_)
  , ncols_(rhs.ncols_)
  , model_(rhs.model_)
  , diag2_(rhs.diag2_)
{
  diag1_ = ClpCopyOfArray(rhs.diag1_, nrows_);
}
// Assignment operator. This copies the data
ClpLsqr &
ClpLsqr::operator=(const ClpLsqr &rhs)
{
  if (this != &rhs) {
    delete[] diag1_;
    diag1_ = ClpCopyOfArray(rhs.diag1_, nrows_);
    nrows_ = rhs.nrows_;
    ncols_ = rhs.ncols_;
    model_ = rhs.model_;
    diag2_ = rhs.diag2_;
  }
  return *this;
}

/* vi: softtabstop=2 shiftwidth=2 expandtab tabstop=2
*/