/*

 * test_shaw.c

 *

 * Test L-curve (Tikhonov) regression routines using Shaw

 * problem. See example 1.10 of

 *

 * [1] R.C. Aster, B. Borchers and C. H. Thurber,

 *     Parameter Estimation and Inverse Problems (2nd ed), 2012.

 */

#include <gsl/gsl_sf_trig.h>

/* alternate (and inefficient) method of computing G(lambda) */

static double

shaw_gcv_G(const double lambda, const gsl_matrix * X, const gsl_vector * y,

           gsl_multifit_linear_workspace * work)

{

  const size_t n = X->size1;

  const size_t p = X->size2;

  gsl_matrix * XTX = gsl_matrix_alloc(p, p);

  gsl_matrix * XI = gsl_matrix_alloc(p, n);

  gsl_matrix * XXI = gsl_matrix_alloc(n, n);

  gsl_vector * c = gsl_vector_alloc(p);

  gsl_vector_view d;

  double rnorm, snorm;

  double term1, term2, G;

  size_t i;

  /* compute regularized solution with this lambda */

  gsl_multifit_linear_solve(lambda, X, y, c, &rnorm, &snorm, work);

  /* compute X^T X */

  gsl_blas_dsyrk(CblasLower, CblasTrans, 1.0, X, 0.0, XTX);

  /* add lambda*I */

  d = gsl_matrix_diagonal(XTX);

  gsl_vector_add_constant(&d.vector, lambda * lambda);

  /* invert (X^T X + lambda*I) */

  gsl_linalg_cholesky_decomp1(XTX);

  gsl_linalg_cholesky_invert(XTX);

  gsl_matrix_transpose_tricpy('L', 0, XTX, XTX);

  /* XI = (X^T X + lambda*I)^{-1} X^T */

  gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, XTX, X, 0.0, XI);

  /* XXI = X (X^T X + lambda*I)^{-1} X^T */

  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, X, XI, 0.0, XXI);

  /* compute: term1 = Tr(I - X XI) */

  term1 = 0.0;

  for (i = 0; i < n; ++i)

    {

      double *Ai = gsl_matrix_ptr(XXI, i, i);

      term1 += 1.0 - (*Ai);

    }

  gsl_matrix_free(XTX);

  gsl_matrix_free(XI);

  gsl_matrix_free(XXI);

  gsl_vector_free(c);

  term2 = rnorm / term1;

  return term2 * term2;;

}

/* construct design matrix and rhs vector for Shaw problem */

static int

shaw_system(gsl_matrix * X, gsl_vector * y)

{

  int s = GSL_SUCCESS;

  const size_t n = X->size1;

  const size_t p = X->size2;

  const double dtheta = M_PI / (double) p;

  size_t i, j;

  gsl_vector *m = gsl_vector_alloc(p);

  /* build the design matrix */

  for (i = 0; i < n; ++i)

    {

      double si = (i + 0.5) * M_PI / n - M_PI / 2.0;

      double csi = cos(si);

      double sni = sin(si);

      for (j = 0; j < p; ++j)

        {

          double thetaj = (j + 0.5) * M_PI / p - M_PI / 2.0;

          double term1 = csi + cos(thetaj);

          double term2 = gsl_sf_sinc(sni + sin(thetaj));

          double Xij = term1 * term1 * term2 * term2 * dtheta;

          gsl_matrix_set(X, i, j, Xij);

        }

    }

  /* construct coefficient vector */

  {

    const double a1 = 2.0;

    const double a2 = 1.0;

    const double c1 = 6.0;

    const double c2 = 2.0;

    const double t1 = 0.8;

    const double t2 = -0.5;

    for (j = 0; j < p; ++j)

      {

        double tj = -M_PI / 2.0 + (j + 0.5) * dtheta;

        double mj = a1 * exp(-c1 * (tj - t1) * (tj - t1)) +

                    a2 * exp(-c2 * (tj - t2) * (tj - t2));

        gsl_vector_set(m, j, mj);

      }

  }

  /* construct rhs vector */

  gsl_blas_dgemv(CblasNoTrans, 1.0, X, m, 0.0, y);

  gsl_vector_free(m);

  return s;

}

static int

test_shaw_system_l(gsl_rng *rng_p, const size_t n, const size_t p,

                   const double lambda_expected,

                   gsl_vector *rhs)

{

  const size_t npoints = 1000; /* number of points on L-curve */

  const double tol1 = 1.0e-12;

  const double tol2 = 1.0e-9;

  const double tol3 = 1.0e-5;

  gsl_vector * reg_param = gsl_vector_alloc(npoints);

  gsl_vector * rho = gsl_vector_alloc(npoints);

  gsl_vector * eta = gsl_vector_alloc(npoints);

  gsl_matrix * X = gsl_matrix_alloc(n, p);

  gsl_matrix * cov = gsl_matrix_alloc(p, p);

  gsl_vector * c = gsl_vector_alloc(p);

  gsl_vector * ytmp = gsl_vector_alloc(n);

  gsl_vector * y;

  gsl_vector * r = gsl_vector_alloc(n);

  gsl_multifit_linear_workspace * work = 

    gsl_multifit_linear_alloc (n, p);

  size_t reg_idx, i;

  double lambda, rnorm, snorm;

  /* build design matrix */

  shaw_system(X, ytmp);

  if (rhs)

    y = rhs;

  else

    {

      y = ytmp;

      /* add random noise to exact rhs vector */

      test_random_vector_noise(rng_p, y);

    }

  /* SVD decomposition */

  gsl_multifit_linear_svd(X, work);

  /* calculate L-curve */

  gsl_multifit_linear_lcurve(y, reg_param, rho, eta, work);

  /* test rho and eta vectors */

  for (i = 0; i < npoints; ++i)

    {

      double rhoi = gsl_vector_get(rho, i);

      double etai = gsl_vector_get(eta, i);

      double lami = gsl_vector_get(reg_param, i);

      /* solve regularized system and check for consistent rho/eta values */

      gsl_multifit_linear_solve(lami, X, y, c, &rnorm, &snorm, work);

      gsl_test_rel(rhoi, rnorm, tol3, "shaw rho n=%zu p=%zu lambda=%e",

                   n, p, lami);

      gsl_test_rel(etai, snorm, tol1, "shaw eta n=%zu p=%zu lambda=%e",

                   n, p, lami);

    }

  /* calculate corner of L-curve */

  gsl_multifit_linear_lcorner(rho, eta, &reg_idx);

  lambda = gsl_vector_get(reg_param, reg_idx);

  /* test against known lambda value if given */

  if (lambda_expected > 0.0)

    {

      gsl_test_rel(lambda, lambda_expected, tol1,

                   "shaw: n=%zu p=%zu L-curve corner lambda",

                   n, p);

    }

  /* compute regularized solution with optimal lambda */

  gsl_multifit_linear_solve(lambda, X, y, c, &rnorm, &snorm, work);

  /* compute residual norm ||y - X c|| */

  gsl_vector_memcpy(r, y);

  gsl_blas_dgemv(CblasNoTrans, 1.0, X, c, -1.0, r);

  /* test rnorm value */

  gsl_test_rel(rnorm, gsl_blas_dnrm2(r), tol2,

               "shaw: n=%zu p=%zu rnorm", n, p);

  /* test snorm value */

  gsl_test_rel(snorm, gsl_blas_dnrm2(c), tol2,

               "shaw: n=%zu p=%zu snorm", n, p);

  gsl_matrix_free(X);

  gsl_matrix_free(cov);

  gsl_vector_free(reg_param);

  gsl_vector_free(rho);

  gsl_vector_free(eta);

  gsl_vector_free(r);

  gsl_vector_free(c);

  gsl_vector_free(ytmp);

  gsl_multifit_linear_free(work);

  return 0;

} /* test_shaw_system_l() */

static int

test_shaw_system_gcv(gsl_rng *rng_p, const size_t n, const size_t p,

                     const double lambda_expected,

                     gsl_vector *rhs)

{

  const size_t npoints = 200; /* number of points on L-curve */

  const double tol1 = 1.0e-12;

  const double tol2 = 1.4e-10;

  const double tol3 = 1.0e-5;

  gsl_vector * reg_param = gsl_vector_alloc(npoints);

  gsl_vector * G = gsl_vector_alloc(npoints);

  gsl_matrix * X = gsl_matrix_alloc(n, p);

  gsl_matrix * cov = gsl_matrix_alloc(p, p);

  gsl_vector * c = gsl_vector_alloc(p);

  gsl_vector * ytmp = gsl_vector_alloc(n);

  gsl_vector * y;

  gsl_vector * r = gsl_vector_alloc(n);

  gsl_multifit_linear_workspace * work = 

    gsl_multifit_linear_alloc (n, p);

  size_t reg_idx, i;

  double lambda, rnorm, snorm, G_lambda;

  /* build design matrix */

  shaw_system(X, ytmp);

  if (rhs)

    y = rhs;

  else

    {

      y = ytmp;

      /* add random noise to exact rhs vector */

      test_random_vector_noise(rng_p, y);

    }

  /* SVD decomposition */

  gsl_multifit_linear_svd(X, work);

  /* calculate GCV curve */

  gsl_multifit_linear_gcv(y, reg_param, G, &lambda, &G_lambda, work);

  /* test G vector */

  for (i = 0; i < npoints; ++i)

    {

      double lami = gsl_vector_get(reg_param, i);

      if (lami > 1.0e-5)

        {

          /* test unreliable for small lambda */

          double Gi = gsl_vector_get(G, i);

          double Gi_expected = shaw_gcv_G(lami, X, y, work);

          gsl_test_rel(Gi, Gi_expected, tol3, "shaw[%zu,%zu] gcv G i=%zu lambda=%e",

                       n, p, i, lami);

        }

    }

  /* test against known lambda value if given */

  if (lambda_expected > 0.0)

    {

      gsl_test_rel(lambda, lambda_expected, tol2,

                   "shaw gcv: n=%zu p=%zu lambda",

                   n, p);

    }

  /* compute regularized solution with optimal lambda */

  gsl_multifit_linear_solve(lambda, X, y, c, &rnorm, &snorm, work);

  /* compute residual norm ||y - X c|| */

  gsl_vector_memcpy(r, y);

  gsl_blas_dgemv(CblasNoTrans, 1.0, X, c, -1.0, r);

  /* test rnorm value */

  gsl_test_rel(rnorm, gsl_blas_dnrm2(r), tol2,

               "shaw gcv: n=%zu p=%zu rnorm", n, p);

  /* test snorm value */

  gsl_test_rel(snorm, gsl_blas_dnrm2(c), tol2,

               "shaw gcv: n=%zu p=%zu snorm", n, p);

  gsl_matrix_free(X);

  gsl_matrix_free(cov);

  gsl_vector_free(reg_param);

  gsl_vector_free(G);

  gsl_vector_free(r);

  gsl_vector_free(c);

  gsl_vector_free(ytmp);

  gsl_multifit_linear_free(work);

  return 0;

} /* test_shaw_system_gcv() */

void

test_shaw(void)

{

  gsl_rng * r = gsl_rng_alloc(gsl_rng_default);

  {

    double shaw20_y[] = {

      8.7547455124379323e-04, 5.4996835885761936e-04,

      1.7527999407005367e-06, 1.9552372913117047e-03,

      1.4411645433785081e-02, 5.2800013336393704e-02,

      1.3609152023257112e-01, 2.7203484587635818e-01,

      4.3752225136193390e-01, 5.7547667319875240e-01,

      6.2445052213539942e-01, 5.6252658286441348e-01,

      4.2322239923561566e-01, 2.6768469219560631e-01,

      1.4337901162734543e-01, 6.5614569346074361e-02,

      2.6013851831752945e-02, 9.2336933089481269e-03,

      3.2269066658993694e-03, 1.3999201459261811e-03 };

    gsl_vector_view rhs = gsl_vector_view_array(shaw20_y, 20);

    /* lambda and rhs values from [1] */

    test_shaw_system_l(r, 20, 20, 5.793190958069266e-06, &rhs.vector);

    test_shaw_system_gcv(r, 20, 20, 1.24921780949051038e-05, &rhs.vector);

  }

  {

    size_t n, p;

    for (n = 10; n <= 50; n += 2)

      {

        for (p = n - 6; p <= n; p += 2)

          test_shaw_system_l(r, n, p, -1.0, NULL);

      }

  }

  gsl_rng_free(r);

} /* test_shaw() */