/* nag_zggrqf (f08ztc) Example Program.
 *
 * Copyright 2011 Numerical Algorithms Group.
 *
 * Mark 23, 2011.
 */

#include <stdio.h>
#include <nag.h>
#include <nag_stdlib.h>
#include <naga02.h>
#include <nagf07.h>
#include <nagf08.h>
#include <nagf16.h>

int main(void)
{
  /* Scalars */
  Complex       alpha, beta;
  double        rnorm;
  Integer       i, j, m, n, p, pda, pdb, pdc, pdd, pdx;
  Integer       y1rows, y2rows, y3rows;
  Integer       exit_status = 0;

  /* Arrays */
  Complex       *a = 0, *b = 0, *c = 0, *d = 0, *taua = 0, *taub = 0, *x = 0;

  /* Nag Types */
  NagError      fail;
  Nag_OrderType order;

#ifdef NAG_COLUMN_MAJOR
#define A(I, J) a[(J-1)*pda + I - 1]
#define B(I, J) b[(J-1)*pdb + I - 1]
  order = Nag_ColMajor;
#else
#define A(I, J) a[(I-1)*pda + J - 1]
#define B(I, J) b[(I-1)*pdb + J - 1]
  order = Nag_RowMajor;
#endif

  INIT_FAIL(fail);

  printf("nag_zggrqf (f08ztc) Example Program Results\n\n");

  /* Skip heading in data file */
  scanf("%*[^\n]");
  scanf("%ld%ld%ld%*[^\n]", &m, &n, &p);
  if (n < 0 || m < 0 || p < 0)
    {
      printf("Invalid n, m or p\n");
      exit_status = 1;
      goto END;
    }

#ifdef NAG_COLUMN_MAJOR
  pda = m;
  pdb = p;
  pdc = m;
  pdd = p;
  pdx = n;
#else
  pda = n;
  pdb = n;
  pdc = 1;
  pdd = 1;
  pdx = 1;
#endif

  /* Allocate memory */
  if (!(a    = NAG_ALLOC(m*n, Complex)) ||
      !(b    = NAG_ALLOC(p*n, Complex)) ||
      !(c    = NAG_ALLOC(m, Complex)) ||
      !(d    = NAG_ALLOC(p, Complex)) ||
      !(taua = NAG_ALLOC(MIN(m, n), Complex)) ||
      !(taub = NAG_ALLOC(MIN(p, n), Complex)) ||
      !(x    = NAG_ALLOC(n, Complex)))
    {
      printf("Allocation failure\n");
      exit_status = -1;
      goto END;
    }

  /* Read A, B, c and d from data file for the problem
   * min{||c-Ax||_2, x in R^n and B x = d}.
   */
  for (i = 1; i <= m; ++i)
    for (j = 1; j <= n; ++j) scanf(" ( %lf , %lf )", &A(i, j).re, &A(i, j).im);
  scanf("%*[^\n]");
  for (i = 1; i <= p; ++i)
    for (j = 1; j <= n; ++j) scanf(" ( %lf , %lf )", &B(i, j).re, &B(i, j).im);
  scanf("%*[^\n]");
  for (i = 0; i < m; ++i) scanf(" ( %lf , %lf )", &c[i].re, &c[i].im);
  scanf("%*[^\n]");
  for (i = 0; i < p; ++i) scanf(" ( %lf , %lf )", &d[i].re, &d[i].im);
  scanf("%*[^\n]");


  /* First compute the generalized RQ factorization of (A,B) as
   * B = (0 R12)*Q,   A = Z*(T11 T12 T13)*Q = T*Q.
   *                        ( 0  T22 T23)
   * where R12, T11 and T22 are upper triangular,
   * using  nag_zggrqf (f08ztc).
   */
  nag_zggrqf(order, p, m, n, b, pdb, taub, a, pda, taua, &fail);
  if (fail.code != NE_NOERROR)
    {
      printf("Error from nag_zggrqf (f08ztc).\n%s\n", fail.message);
      exit_status = 1;
      goto END;
    }

  /* Now, Z^H * (c-Ax) = Z^H * c - T*Q*x, and 
   * let f = (f1) = Z^H * (c1) => minimize ||f - T*Q*x||
   *         (f2)         (c2)
   * Compute f using  nag_zunmqr (f08auc), storing result in c 
   */
  nag_zunmqr(order, Nag_LeftSide, Nag_ConjTrans, m, 1, MIN(m, n), a, pda, taua,
	     c, pdc, &fail);
  if (fail.code != NE_NOERROR)
    {
      printf("Error from nag_zunmqr (f08auc).\n%s\n", fail.message);
      exit_status = 1;
      goto END;
    }

  /* Putting Q*x = (y1), B * x = d becomes (0 R12) (y1) = d;
   *               (w )                            (w )
   * => R12 * w = d.
   * Solve for w using nag_dtrtrs (f07tec), storing result in d;
   * R12 is (p by p) triangular submatrix starting at B(1,n-p+1).
   */
  nag_ztrtrs(order, Nag_Upper, Nag_NoTrans, Nag_NonUnitDiag, p, 1,
	     &B(1, n - p + 1), pdb, d, pdd, &fail);
  if (fail.code != NE_NOERROR)
    {
      printf("Error from nag_ztrtrs (f07tsc).\n%s\n", fail.message);
      exit_status = 1;
      goto END;
    }

  /* The problem now reduces to finding the minimum norm of
   * g = (g1) = (f1) - T11*y1 - (T12 T13)*w
   *     (g2)   (f2)          - (T22 T23)*w.
   * Form c1 = f1 - (T12 T13)*w using  nag_zgemv (f16sac).
   */
  alpha = nag_complex(-1.0,0.0);
  beta = nag_complex(1.0,0.0);
  y1rows = n - p;
  nag_zgemv(order, Nag_NoTrans, y1rows, p, alpha, &A(1, n-p+1), pda, d, 1,
	    beta, c, 1, &fail);
  if (fail.code != NE_NOERROR)
    {
      printf("Error from nag_zgemv (f16sac).\n%s\n", fail.message);
      exit_status = 1;
      goto END;
    }
   /* => now (g1) = c - T11*y1 and ||g1|| = 0 when T11 * y1 = c1.
    * Solve this for y1 using nag_ztrtrs (f07tsc) storing result in c1.
    */
  nag_ztrtrs(order, Nag_Upper, Nag_NoTrans, Nag_NonUnitDiag, y1rows, 1, a, pda,
	     c, pdc, &fail);
  if (fail.code != NE_NOERROR)
    {
      printf("Error from nag_ztrtrs (f07tsc).\n%s\n", fail.message);
      exit_status = 1;
      goto END;
    }

  /* So now Q*x = (y1) is stored in (c1), which is now copied to x.
   *              (w )              (d )
   */
  for (i = 0; i < y1rows; ++i) x[i] = nag_complex(c[i].re,c[i].im);
  for (i = 0; i < n-y1rows; ++i) x[y1rows+i] = nag_complex(d[i].re,d[i].im);

  /* Compute x by applying Q inverse using nag_zunmrq (f08cxc). */
  nag_zunmrq(order, Nag_LeftSide, Nag_ConjTrans, n, 1, p, b, pdb, taub, x, pdx,
	     &fail);
  if (fail.code != NE_NOERROR)
    {
      printf("Error from nag_zunmrq (f08cxc).\n%s\n", fail.message);
      exit_status = 1;
      goto END;
    }

  /* It remains to minimize  ||g2||, g2 = f2 - (T22 T23)*w.
   * Putting w = (y2), gives g2 = f2 - T22*y2 - T23*y3
   *             (y3)
   * [y2 stored in d1, first y2rows of d; y3 stored in d2, next n-m rows of d.]
   *
   * First form T22*y2 using  nag_ztrmv (f16sfc) where y2 is held in d.
   */
  y2rows = MIN(m, n) - y1rows;
  alpha = nag_complex(1.0,0.0);
  nag_ztrmv(order, Nag_Upper, Nag_NoTrans, Nag_NonUnitDiag, y2rows, alpha,
	    &A(n-p+1, n-p+1), pda, d, 1, &fail);
  if (fail.code != NE_NOERROR)
    {
      printf("Error from nag_ztrmv (f16sfc).\n%s\n", fail.message);
      exit_status = 1;
      goto END;
    }

  /* Then, f2 - T22*y2 (c2 = c2 - d) */
  for (i = 0; i < y2rows; ++i)
    c[y1rows + i] = nag_complex_subtract(c[y1rows+i], d[i]);
  y2rows = m - y1rows;

  if (m < n)
    {
      y3rows = n - m;
      /* Then g2 = f2 - T22*y2 - T23*y3 (c2 = c2 - T23*d2) */
      alpha = nag_complex(-1.0,0.0);
      nag_zgemv(order, Nag_NoTrans, y2rows, y3rows, alpha, &A(n-p+1, m+1), pda,
		&d[y2rows], 1, beta, &c[y1rows], 1, &fail);
      if (fail.code != NE_NOERROR)
	{
	  printf("Error from nag_zgemv (f16sac).\n%s\n", fail.message);
	  exit_status = 1;
	  goto END;
	}
    }

  /* Compute ||g|| = ||g2|| = norm(f2 - T22*y2 - T23*y3)
   * using nag_zge_norm (f16uac).
   */
  nag_zge_norm(Nag_ColMajor, Nag_FrobeniusNorm, y2rows, 1, &c[y1rows], y2rows,
	       &rnorm, &fail);
  if (fail.code != NE_NOERROR)
    {
      printf("Error from nag_zge_norm (f16uac).\n%s\n", fail.message);
      exit_status = 1;
      goto END;
    }

  /* Print least squares solution x */
  printf("Constrained least squares solution\n");
  for (i = 0; i < n; ++i)
    printf(" (%7.4f, %7.4f)%s", x[i].re, x[i].im, i%4 == 3?"\n":"");

  /* Print the square root of the residual sum of squares */
  printf("\nSquare root of the residual sum of squares\n");
  printf("%11.2e\n", rnorm);

 END:
  if (a)    NAG_FREE(a);
  if (b)    NAG_FREE(b);
  if (c)    NAG_FREE(c);
  if (d)    NAG_FREE(d);
  if (taua) NAG_FREE(taua);
  if (taub) NAG_FREE(taub);
  if (x)    NAG_FREE(x);

  return exit_status;
}