/*
Copyright(C) 2007-2012 National Institute of Information and Communications Technology

This code, which implements L-BFGS optimization algorithm, is translated into C by hand
from the original Fortran code.
The original Fortran code is written by Prof. Jorge Nocedal and released under the BSD license.
*/

/*
  Software for Large-scale Unconstrained Optimization
  L-BFGS is a limited-memory quasi-Newton code for unconstrained
  optimization.

  http://www.ece.northwestern.edu/~nocedal/lbfgs.html

  Authors
  Jorge Nocedal

  References
  - J. Nocedal. Updating Quasi-Newton Matrices with Limited Storage(1980),
    Mathematics of Computation 35, pp. 773-782.
  - D.C. Liu and J. Nocedal. On the Limited Memory Method for
    Large Scale Optimization(1989),  Mathematical Programming B, 45, 3, pp. 503-528.
*/

#include <stdio.h>
#include <math.h>
#include "lbfgs.h"


#define MOD(x, y) ((x) % (y))
#define MIN(x, y) (((x) <= (y)) ? (x) : (y))
#define MAX(x, y) (((x) >= (y)) ? (x) : (y))
#define MAX3(x, y, z) (((x) >= (y)) ? MAX((x), (z)) : MAX((y), (z)))
#define TRUE 1
#define FALSE 0


static int LP = 6;
static double GTOL = 9.0e-02, STPMIN = 1.0e-20, STPMAX = 1.0e+20;


static double DDOT(int n, double *dx, int incx, double *dy, int incy);
static void LB1(int *IPRINT, int ITER, int NFUN, double GNORM, int N, int M, double *X, double F, double *G, double STP, int FINISH);
static void MCSRCH(int N, double *X, double F, double *G, double *S, double (*STP), double FTOL, double XTOL, int MAXFEV, int (*INFO), int (*NFEV), double *WA);
static void MCSTEP(double (*STX), double (*FX), double (*DX), double (*STY), double (*FY), double (*DY), double (*STP), double FP, double DP, int (*BRACKT), double STPMIN, double STPMAX, int (*INFO));
static void DAXPY(int n, double da, double *dx, int incx, double *dy, int incy);


/*
*/
void lbfgs(int N, int M, double *X, double F, double *G, int DIAGCO, double *DIAG, int *IPRINT, double EPS, double XTOL, double *W, int (*IFLAG)) {
  static double ONE = 1.0e+0, ZERO = 0.0e+0, GNORM, STP1, FTOL, STP, YS, YY, SQ, YR, BETA, XNORM;
  static int ITER, NFUN, POINT, ISPT, IYPT, MAXFEV, INFO, BOUND, NPT, CP, I, NFEV, INMC, IYCN, ISCN;
  static int FINISH;
  X--; G--; DIAG--; IPRINT--; W--;

  if (*IFLAG == 0) goto l10;
  if (*IFLAG == 1) goto l172; else if (*IFLAG == 2) goto l100;
 l10:
  ITER = 0;
  if (N <= 0 || M <= 0) goto l196;
  if (GTOL <= 1.0e-04) {
    if (LP > 0) fprintf(stderr, "\n  GTOL IS LESS THAN OR EQUAL TO 1.D-04\n IT HAS BEEN RESET TO 9.D-01" "\n");
    GTOL = 9.0e-01;
  }
  NFUN = 1;
  POINT = 0;
  FINISH = FALSE;
  if (DIAGCO) {
    for (I = 1; I <= N; I++) {
      if (DIAG[I] <= ZERO) goto l195;
    }
  } else {
    for (I = 1; I <= N; I++) {
      DIAG[I] = 1.0e0;
    }
  }

  ISPT = N + 2 * M;
  IYPT = ISPT + N * M;
  for (I = 1; I <= N; I++) {
    W[ISPT + I] = -G[I] * DIAG[I];
  }
  GNORM = sqrt(DDOT(N, &(G[1]), 1, &(G[1]), 1));
  STP1 = ONE / GNORM;

  FTOL = 1.0e-4;
  MAXFEV = 20;

  if (IPRINT[1] >= 0) LB1(&(IPRINT[1]), ITER, NFUN, GNORM, N, M, &(X[1]), F, &(G[1]), STP, FINISH);

 l80:
  ITER = ITER + 1;
  INFO = 0;
  BOUND = ITER - 1;
  if (ITER == 1) goto l165;
  if (ITER > M) BOUND = M;

  YS = DDOT(N, &(W[IYPT + NPT + 1]), 1, &(W[ISPT + NPT + 1]), 1);
  if (!DIAGCO) {
    YY = DDOT(N, &(W[IYPT + NPT + 1]), 1, &(W[IYPT + NPT + 1]), 1);
    for (I=1; I<= N; I++) {
      DIAG[I] = YS / YY;
    }
  } else {
    *IFLAG = 2;
    return;
  }

 l100:
  if (DIAGCO) {
    for (I = 1; I <= N; I++) {
      if (DIAG[I] <= ZERO) goto l195;
    }
  }

  CP = POINT;
  if (POINT == 0) CP = M;
  W[N + CP] = ONE / YS;
  for (I = 1; I <= N; I++) {
    W[I] = -G[I];
  }
  CP = POINT;
  for (I = 1; I <= BOUND; I++) {
    CP = CP - 1;
    if (CP == -1) CP = M - 1;
    SQ = DDOT(N, &(W[ISPT + CP * N + 1]), 1, &(W[1]), 1);
    INMC = N + M + CP + 1;
    IYCN = IYPT + CP * N;
    W[INMC] = W[N + CP + 1] * SQ;
    DAXPY(N, -W[INMC], &(W[IYCN + 1]), 1, &(W[1]), 1);
  }

  for (I = 1; I <= N; I++) {
    W[I] = DIAG[I] * W[I];
  }

  for (I = 1; I <= BOUND; I++) {
    YR = DDOT(N, &(W[IYPT + CP * N + 1]), 1, &(W[1]), 1);
    BETA = W[N + CP + 1] * YR;
    INMC = N + M + CP + 1;
    BETA = W[INMC] - BETA;
    ISCN = ISPT + CP * N;
    DAXPY(N, BETA, &(W[ISCN + 1]), 1, &(W[1]), 1);
    CP = CP + 1;
    if (CP == M) CP = 0;
  }

  for (I = 1; I <= N; I++) {
    W[ISPT + POINT * N + I] = W[I];
  }

 l165:
  NFEV = 0;
  STP = ONE;
  if (ITER == 1) STP = STP1;
  for (I = 1; I <= N; I++) {
    W[I] = G[I];
  }

 l172:

  MCSRCH(N, &(X[1]), F, &(G[1]), &(W[ISPT + POINT * N + 1]), &STP, FTOL, XTOL, MAXFEV, &INFO, &NFEV, &(DIAG[1]));
  if (INFO == -1) {
    *IFLAG = 1;
    return;
  }
  if (INFO != 1) goto l190;
  NFUN = NFUN + NFEV;

  NPT = POINT * N;
  for (I = 1; I <= N; I++) {
    W[ISPT + NPT + I] = STP * W[ISPT + NPT + I];
    W[IYPT + NPT + I] = G[I] - W[I];
  }
  POINT = POINT + 1;
  if (POINT == M) POINT = 0;

  GNORM = sqrt(DDOT(N, &(G[1]), 1, &(G[1]), 1));
  XNORM = sqrt(DDOT(N, &(X[1]), 1, &(X[1]), 1));
  XNORM = MAX(1.0e0, XNORM);
  if (GNORM / XNORM <= EPS) FINISH = TRUE;

  if (IPRINT[1] >= 0) LB1(&(IPRINT[1]), ITER, NFUN, GNORM, N, M, &(X[1]), F, &(G[1]), STP, FINISH);
  if (FINISH) {
    *IFLAG = 0;
    return;
  }
  goto l80;

 l190:
  *IFLAG = -1;
  if (LP > 0) fprintf(stderr, "\n IFLAG= -1 \n LINE SEARCH FAILED. SEE DOCUMENTATION OF ROUTINE MCSRCH\n ERROR RETURN OF LINE SEARCH: INFO= %2d\n POSSIBLE CAUSES: FUNCTION OR GRADIENT ARE INCORRECT\n OR INCORRECT TOLERANCES" "\n", INFO);
  return;
 l195:
  *IFLAG = -2;
  if (LP > 0) fprintf(stderr, "\n IFLAG= -2\n THE%5d-TH DIAGONAL ELEMENT OF THE\n INVERSE HESSIAN APPROXIMATION IS NOT POSITIVE" "\n", I);
  return;
  l196:
  *IFLAG = -3;
  if (LP > 0) fprintf(stderr, "\n IFLAG= -3\n IMPROPER INPUT PARAMETERS (N OR M ARE NOT POSITIVE)" "\n");
  return;
}


/*
*/
static void LB1(int *IPRINT, int ITER, int NFUN, double GNORM, int N, int M, double *X, double F, double *G, double STP, int FINISH) {
  static int I;
  IPRINT--; X--; G--;

  if (ITER == 0) {
    fprintf(stderr, "*************************************************" "\n");
    fprintf(stderr, "  N=%5d   NUMBER OF CORRECTIONS=%2d\n       INITIAL VALUES" "\n", N, M);
    fprintf(stderr, " F= %10.3E   GNORM= %10.3E" "\n", F, GNORM);
    if (IPRINT[2] >= 1) {
      fprintf(stderr, " VECTOR X= " "\n");
      for (I = 1; I <= N; I++) fprintf(stderr, "  %10.3E%s", X[I], (I % 6 == 0 || I == N) ? "\n" : "");
      fprintf(stderr, " GRADIENT VECTOR G= " "\n")/*60*/;
      for (I = 1; I <= N; I++) fprintf(stderr, "  %10.3E%s", G[I], (I % 6 == 0 || I == N) ? "\n" : "");
    }
    fprintf(stderr, "*************************************************" "\n");
    fprintf(stderr, "\n   I   NFN    FUNC        GNORM       STEPLENGTH\n" "\n");
  } else {
    if ((IPRINT[1] == 0) && (ITER != 1 && !FINISH)) return;
    if (IPRINT[1] != 0) {
      if (MOD(ITER - 1, IPRINT[1]) == 0 || FINISH) {
	if (IPRINT[2] > 1 && ITER > 1) fprintf(stderr, "\n   I   NFN    FUNC        GNORM       STEPLENGTH\n" "\n");

	fprintf(stderr, "%4d %4d    %10.3E  %10.3E  %10.3E" "\n", ITER, NFUN, F, GNORM, STP);
      } else {
	return;
      }
    } else {
      if (IPRINT[2] > 1 && FINISH) fprintf(stderr, "\n   I   NFN    FUNC        GNORM       STEPLENGTH\n" "\n");
      fprintf(stderr, "%4d %4d    %10.3E  %10.3E  %10.3E" "\n", ITER, NFUN, F, GNORM, STP);
    }
    if (IPRINT[2] == 2 || IPRINT[2] == 3) {
      if (FINISH) {
	fprintf(stderr, " FINAL POINT X= " "n");
      } else {
	fprintf(stderr, " VECTOR X= " "\n");
      }
      for (I = 1; I <= N; I++) fprintf(stderr, "  %10.3E%s", X[I], (I % 6 == 0 || I == N) ? "\n" : "");
      if (IPRINT[2] == 3) {
	fprintf(stderr, " GRADIENT VECTOR G= " "\n")/*60*/;
	for (I = 1; I <= N; I++) fprintf(stderr, "  %10.3E%s", G[I], (I % 6 == 0 || I == N) ? "\n" : "");
      }
    }
    if (FINISH) fprintf(stderr, "\n THE MINIMIZATION TERMINATED WITHOUT DETECTING ERRORS.\n IFLAG = 0" "\n");
  }

  return;
}


/*
*/
static void DAXPY(int n, double da, double *dx, int incx, double *dy, int incy) {
  static int i, ix, iy, m, mp1;
  dx--; dy--;

  if (n <= 0) return;
  if (da == 0.0e0) return;
  if (incx == 1 && incy == 1) goto l20;

  ix = 1;
  iy = 1;
  if (incx < 0) ix = (-n + 1) * incx + 1;
  if (incy < 0) iy = (-n + 1) * incy + 1;
  for (i = 1; i <= n; i++) {
    dy[iy] = dy[iy] + da * dx[ix];
    ix = ix + incx;
    iy = iy + incy;
  }
  return;

 l20:
  m = MOD(n, 4);
  if (m == 0) goto l40;
  for (i = 1; i <= m; i++) {
    dy[i] = dy[i] + da * dx[i];
  }
  if (n < 4 ) return;
 l40:
  mp1 = m + 1;
  for (i = mp1; i <= n; i += 4) {
    dy[i    ] = dy[i    ] + da * dx[i    ];
    dy[i + 1] = dy[i + 1] + da * dx[i + 1];
    dy[i + 2] = dy[i + 2] + da * dx[i + 2];
    dy[i + 3] = dy[i + 3] + da * dx[i + 3];
  }
  return;
}


/*
*/
static double DDOT(int n, double *dx, int incx, double *dy, int incy) {
  static double dtemp;
  static int i, ix, iy, m, mp1;
  static double result;
  dx--; dy--;

  result = 0.0e0;
  dtemp = 0.0e0;
  if (n <= 0) return result;
  if (incx == 1 && incy == 1) goto l20;
  ix = 1;
  iy = 1;
  if (incx < 0) ix = (-n + 1) * incx + 1;
  if (incy < 0) iy = (-n + 1) * incy + 1;
  for (i = 1; i <= n; i++) {
    dtemp = dtemp + dx[ix] * dy[iy];
    ix = ix + incx;
    iy = iy + incy;
  }
  result = dtemp;
  return result;

 l20:
  m = MOD(n, 5);
  if (m == 0) goto l40;
  for (i = 1; i <= m; i++) {
    dtemp = dtemp + dx[i] * dy[i];
  }
  if (n < 5) goto l60;
 l40:
  mp1 = m + 1;
  for (i = mp1; i <= n; i += 5) {
    dtemp = dtemp + dx[i] * dy[i] + dx[i + 1] * dy[i + 1] + dx[i + 2] * dy[i + 2] + dx[i + 3] * dy[i + 3] + dx[i + 4] * dy[i + 4];
  }
 l60:
  result = dtemp;
  return result;
}


/*
*/
static void MCSRCH(int N, double *X, double F, double *G, double *S, double (*STP), double FTOL, double XTOL, int MAXFEV, int (*INFO), int (*NFEV), double *WA) {
  static int INFOC, J;
  static int BRACKT, STAGE1;
  static double DG, DGM, DGINIT, DGTEST, DGX, DGXM, DGY, DGYM, FINIT, FTEST1, FM, FX, FXM, FY, FYM ,P5 = 0.5e0, P66 = 0.66e0, STX, STY, STMIN, STMAX, WIDTH, WIDTH1, XTRAPF = 4.0e0, ZERO = 0.0e0;
  X--; G--; S--; WA--;

  if (*INFO == -1) goto l45;
  INFOC = 1;

  if (N <= 0 || *STP <= ZERO || FTOL < ZERO || GTOL < ZERO || XTOL < ZERO || STPMIN < ZERO || STPMAX < STPMIN || MAXFEV <= 0) return;

  DGINIT = ZERO;
  for (J = 1; J <= N; J++) {
    DGINIT = DGINIT + G[J] * S[J];
  }
  if (DGINIT >= ZERO) {
    fprintf(stderr, "\n  THE SEARCH DIRECTION IS NOT A DESCENT DIRECTION" "\n");
    return;
  }

  BRACKT = FALSE;
  STAGE1 = TRUE;
  *NFEV = 0;
  FINIT = F;
  DGTEST = FTOL * DGINIT;
  WIDTH = STPMAX - STPMIN;
  WIDTH1 = WIDTH / P5;
  for (J = 1; J <= N; J++) {
    WA[J] = X[J];
  }

  STX = ZERO;
  FX = FINIT;
  DGX = DGINIT;
  STY = ZERO;
  FY = FINIT;
  DGY = DGINIT;

 l30:
  if (BRACKT) {
    STMIN = MIN(STX, STY);
    STMAX = MAX(STX, STY);
  } else {
    STMIN = STX;
    STMAX = *STP + XTRAPF * (*STP - STX);
  }

  *STP = MAX(*STP, STPMIN);
  *STP = MIN(*STP, STPMAX);

  if ((BRACKT && (*STP <= STMIN || *STP >= STMAX)) || *NFEV >= MAXFEV - 1 || INFOC == 0 || (BRACKT && STMAX-STMIN <= XTOL * STMAX)) *STP = STX;

  for (J = 1; J <= N; J++) {
    X[J] = WA[J] + *STP * S[J];
  }
  *INFO = -1;
  return;

 l45:
  *INFO = 0;
  *NFEV = *NFEV + 1;
  DG = ZERO;
  for (J = 1; J <= N; J++) {
    DG = DG + G[J] * S[J];
  }
  FTEST1 = FINIT + *STP * DGTEST;

  if ((BRACKT && (*STP <= STMIN || *STP >= STMAX)) || INFOC == 0) *INFO = 6;
  if (*STP == STPMAX && F <= FTEST1 && DG <= DGTEST) *INFO = 5;
  if (*STP == STPMIN && (F > FTEST1 || DG >= DGTEST)) *INFO = 4;
  if (*NFEV >= MAXFEV) *INFO = 3;
  if (BRACKT && STMAX-STMIN <= XTOL * STMAX) *INFO = 2;
  if (F <= FTEST1 && fabs(DG) <= GTOL * (-DGINIT)) *INFO = 1;

  if (*INFO != 0) return;

  if (STAGE1 && F <= FTEST1 && DG >= MIN(FTOL, GTOL) * DGINIT) STAGE1 = FALSE;

  if (STAGE1 && F <= FX && F > FTEST1) {
    FM = F - *STP * DGTEST;
    FXM = FX - STX * DGTEST;
    FYM = FY - STY * DGTEST;
    DGM = DG - DGTEST;
    DGXM = DGX - DGTEST;
    DGYM = DGY - DGTEST;

    MCSTEP(&STX, &FXM, &DGXM, &STY, &FYM, &DGYM, STP, FM, DGM, &BRACKT, STMIN, STMAX, &INFOC);

    FX = FXM + STX * DGTEST;
    FY = FYM + STY * DGTEST;
    DGX = DGXM + DGTEST;
    DGY = DGYM + DGTEST;
  } else {
    MCSTEP(&STX, &FX, &DGX, &STY, &FY, &DGY, STP, F, DG, &BRACKT, STMIN, STMAX, &INFOC);
  }

  if (BRACKT) {
    if (fabs(STY - STX) >= P66 * WIDTH1) *STP = STX + P5 * (STY - STX);
    WIDTH1 = WIDTH;
    WIDTH = fabs(STY - STX);
  }

  goto l30;
}


/*
*/
static void MCSTEP(double (*STX), double (*FX), double (*DX), double (*STY), double (*FY), double (*DY), double (*STP), double FP, double DP, int (*BRACKT), double STPMIN, double STPMAX, int (*INFO)) {
  static int BOUND;
  static double GAMMA, P, Q, R, S, SGND, STPC, STPF, STPQ, THETA;

  *INFO = 0;

  if ((*BRACKT && (*STP <= MIN(*STX, *STY) || *STP >= MAX(*STX, *STY))) || *DX * (*STP - *STX) >= 0.0 || STPMAX < STPMIN) return;

  SGND = DP * (*DX / fabs(*DX));

  if (FP > *FX) {
    *INFO = 1;
    BOUND = TRUE;
    THETA = 3 * (*FX - FP) / (*STP - *STX) + *DX + DP;
    S = MAX3(fabs(THETA), fabs(*DX), fabs(DP));
    GAMMA = S * sqrt((THETA / S) * (THETA / S) - (*DX / S) * (DP / S));
    if (*STP < *STX) GAMMA = -GAMMA;
    P = (GAMMA - *DX) + THETA;
    Q = ((GAMMA - *DX) + GAMMA) + DP;
    R = P / Q;
    STPC = *STX + R * (*STP - *STX);
    STPQ = *STX + ((*DX / ((*FX - FP)/(*STP - *STX) + *DX)) / 2) * (*STP - *STX);
    if (fabs(STPC - *STX) < fabs(STPQ - *STX)) {
      STPF = STPC;
    } else {
      STPF = STPC + (STPQ - STPC) / 2;
    }
    *BRACKT = TRUE;
  } else if (SGND < 0.0) {
    *INFO = 2;
    BOUND = FALSE;
    THETA = 3 * (*FX - FP) / (*STP - *STX) + *DX + DP;
    S = MAX3(fabs(THETA), fabs(*DX), fabs(DP));
    GAMMA = S * sqrt((THETA / S) * (THETA / S) - (*DX / S) * (DP / S));
    if (*STP > *STX) GAMMA = -GAMMA;
    P = (GAMMA - DP) + THETA;
    Q = ((GAMMA - DP) + GAMMA) + *DX;
    R = P / Q;
    STPC = *STP + R * (*STX - *STP);
    STPQ = *STP + (DP / (DP - *DX)) * (*STX - *STP);
    if (fabs(STPC - *STP) > fabs(STPQ - *STP)) {
      STPF = STPC;
    } else {
      STPF = STPQ;
    }
    *BRACKT = TRUE;
  } else if (fabs(DP) < fabs(*DX)) {
    *INFO = 3;
    BOUND = TRUE;
    THETA = 3 * (*FX - FP) / (*STP - *STX) + *DX + DP;
    S = MAX3(fabs(THETA), fabs(*DX), fabs(DP));
    GAMMA = S * sqrt(MAX(0.0e0, (THETA / S) * (THETA / S) - (*DX / S) * (DP / S)));
    if (*STP > *STX) GAMMA = -GAMMA;
    P = (GAMMA - DP) + THETA;
    Q = (GAMMA + (*DX - DP)) + GAMMA;
    R = P / Q;
    if (R < 0.0 && GAMMA != 0.0) {
      STPC = *STP + R * (*STX - *STP);
    } else if (*STP > *STX) {
      STPC = STPMAX;
    } else {
      STPC = STPMIN;
    }
    STPQ = *STP + (DP / (DP - *DX)) * (*STX - *STP);
    if (*BRACKT) {
      if (fabs(*STP - STPC) < fabs(*STP - STPQ)) {
	STPF = STPC;
      } else {
	STPF = STPQ;
      }
    } else {
      if (fabs(*STP - STPC) > fabs(*STP - STPQ)) {
	STPF = STPC;
      } else {
	STPF = STPQ;
      }
    }
  } else {
    *INFO = 4;
    BOUND = FALSE;
    if (*BRACKT) {
      THETA = 3 * (FP - *FY) / (*STY - *STP) + *DY + DP;
      S = MAX3(fabs(THETA), fabs(*DY), fabs(DP));
      GAMMA = S * sqrt((THETA / S) * (THETA / S) - (*DY / S) * (DP / S));
      if (*STP > *STY) GAMMA = -GAMMA;
      P = (GAMMA - DP) + THETA;
      Q = ((GAMMA - DP) + GAMMA) + *DY;
      R = P / Q;
      STPC = *STP + R * (*STY - *STP);
      STPF = STPC;
    } else if (*STP > *STX) {
      STPF = STPMAX;
    } else {
      STPF = STPMIN;
    }
  }

  if (FP > *FX) {
    *STY = *STP;
    *FY = FP;
    *DY = DP;
  } else {
    if (SGND < 0.0) {
      *STY = *STX;
      *FY = *FX;
      *DY = *DX;
    }
    *STX = *STP;
    *FX = FP;
    *DX = DP;
  }

  STPF = MIN(STPMAX, STPF);
  STPF = MAX(STPMIN, STPF);
  *STP = STPF;
  if (*BRACKT && BOUND) {
    if (*STY > *STX) {
      *STP = MIN(*STX + 0.66 * (*STY - *STX), *STP);
    } else {
      *STP = MAX(*STX + 0.66 * (*STY - *STX), *STP);
    }
  }
  return;
}
