/* nag_zuncsd (f08rnc) Example Program.
 *
 * Copyright 2017 Numerical Algorithms Group.
 *
 * Mark 26.1, 2017.
 */

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

int main(void)
{
  /* Scalars */
  Integer exit_status = 0;
  Integer pdx, pdu, pdv, pdx11, pdx12, pdx21, pdx22, pdw;
  Integer i, j, m, p, q, n11, n12, n21, n22, r;
  Integer recombine = 0, reprint = 0;
  Complex cone = { 1.0, 0.0 }, czero = {
  0.0, 0.0};
  /* Arrays */
  Complex *u = 0, *u1 = 0, *u2 = 0, *v = 0, *v1t = 0, *v2t = 0, *w = 0,
         *x = 0, *x11 = 0, *x12 = 0, *x21 = 0, *x22 = 0;
  double *theta = 0;
  /* Nag Types */
  Nag_OrderType order;
  NagError fail;

#ifdef NAG_COLUMN_MAJOR
#define X(I,J) x[(J-1)*pdx + I-1]
#define U(I,J) u[(J-1)*pdu + I-1]
#define V(I,J) v[(J-1)*pdv + I-1]
#define W(I,J) w[(J-1)*pdw + I-1]
#define X11(I,J) x11[(J-1)*pdx11 + I-1]
#define X12(I,J) x12[(J-1)*pdx12 + I-1]
#define X21(I,J) x21[(J-1)*pdx21 + I-1]
#define X22(I,J) x22[(J-1)*pdx22 + I-1]
  order = Nag_ColMajor;
#else
#define X(I,J) x[(I-1)*pdx + J-1]
#define U(I,J) u[(I-1)*pdu + J-1]
#define V(I,J) v[(I-1)*pdv + J-1]
#define W(I,J) w[(I-1)*pdw + J-1]
#define X11(I,J) x11[(I-1)*pdx11 + J-1]
#define X12(I,J) x12[(I-1)*pdx12 + J-1]
#define X21(I,J) x21[(I-1)*pdx21 + J-1]
#define X22(I,J) x22[(I-1)*pdx22 + J-1]
  order = Nag_RowMajor;
#endif

  INIT_FAIL(fail);

  printf("nag_zuncsd (f08rnc) Example Program Results\n\n");
  fflush(stdout);

  /* Skip heading in data file */
  scanf("%*[^\n] ");
  scanf("%" NAG_IFMT "%" NAG_IFMT "%" NAG_IFMT "%*[^\n] ", &m, &p, &q);

  r = MIN(MIN(p, q), MIN(m - p, m - q));

  if (!(x = NAG_ALLOC(m * m, Complex)) ||
      !(u = NAG_ALLOC(m * m, Complex)) ||
      !(v = NAG_ALLOC(m * m, Complex)) ||
      !(w = NAG_ALLOC(m * m, Complex)) ||
      !(theta = NAG_ALLOC(r, double)) ||
      !(x11 = NAG_ALLOC(p * q, Complex)) ||
      !(x12 = NAG_ALLOC(p * (m - q), Complex)) ||
      !(x21 = NAG_ALLOC((m - p) * q, Complex)) ||
      !(x22 = NAG_ALLOC((m - p) * (m - q), Complex)) ||
      !(u1 = NAG_ALLOC(p * p, Complex)) ||
      !(u2 = NAG_ALLOC((m - p) * (m - p), Complex)) ||
      !(v1t = NAG_ALLOC(q * q, Complex)) ||
      !(v2t = NAG_ALLOC((m - q) * (m - q), Complex)))
  {
    printf("Allocation failure\n");
    exit_status = -1;
    goto END;
  }
  pdx = m;
  pdu = m;
  pdv = m;
  pdw = m;
#ifdef NAG_COLUMN_MAJOR
  pdx11 = p;
  pdx12 = p;
  pdx21 = m - p;
  pdx22 = m - p;
#else
  pdx11 = q;
  pdx12 = m - q;
  pdx21 = q;
  pdx22 = m - q;
#endif

  /* Read (by column) and print unitary X from data file
   * (as, say, generated by a generalized singular value decomposition).
   */
  for (i = 1; i <= m; i++) {
    for (j = 1; j <= m; j++)
      scanf(" ( %lf , %lf ) ", &X(j, i).re, &X(j, i).im);
    scanf("%*[^\n] ");
  }
  /* Store partitions of X in separate matrices */
  for (j = 1; j <= p; j++) {
    for (i = 1; i <= q; i++)
      X11(j, i) = X(j, i);
    for (i = 1; i <= m - q; i++)
      X12(j, i) = X(j, i + q);
  }
  for (j = 1; j <= m - p; j++) {
    for (i = 1; i <= q; i++)
      X21(j, i) = X(j + p, i);
    for (i = 1; i <= m - q; i++)
      X22(j, i) = X(j + p, i + q);
  }

  /* nag_gen_complx_mat_print_comp (x04dbc).
   * Print least squares solutions.
   */
  nag_gen_complx_mat_print_comp(order, Nag_GeneralMatrix, Nag_NonUnitDiag, m,
                                m, x, pdx, Nag_BracketForm, "%7.4f",
                                "Unitary matrix X", Nag_IntegerLabels,
                                0, Nag_IntegerLabels, 0, 80, 0, 0, &fail);
  if (fail.code != NE_NOERROR) {
    printf("Error from nag_gen_complx_mat_print_comp (x04dbc).\n%s\n",
           fail.message);
    exit_status = 1;
    goto END;
  }
  printf("\n");
  fflush(stdout);

  /* nag_zuncsd (f08rnc).
   * Compute the complete CS factorization of X:
   * X11 is stored in X(1:p,    1:q),  X12 is stored in X(1:p,   q+1:m)
   * X21 is stored in X(p+1:m,  1:q),  X22 is stored in X(p+1:m, q+1:m)
   * U1  is stored in U(1:p,    1:p),  U2  is stored in U(p+1:m, p+1:m)
   * V1  is stored in V(1:q,    1:q),  V2  is stored in V(q+1:m, q+1:m)
   */

  /* This is how you might pass partitions as sub-matrices */
  nag_zuncsd(order, Nag_AllU, Nag_AllU, Nag_AllVT, Nag_AllVT, Nag_UpperMinus,
             m, p, q, x, pdx, &X(1, q + 1), pdx, &X(p + 1, 1), pdx, &X(p + 1,
                                                                       q + 1),
             pdx, theta, u, pdu, &U(p + 1, p + 1), pdu, v, pdv, &V(q + 1,
                                                                   q + 1),
             pdv, &fail);
  if (fail.code != NE_NOERROR) {
    printf("Error from nag_zuncsd (f08rnc).\n%s\n", fail.message);
    exit_status = 2;
    goto END;
  }

  /* Print Theta using matrix printing routine
   * nag_gen_real_mat_print (x04cac).
   * Note: U1, U2, V1T, V2T not printed since these may differ by a sign
   * change in columns of U1, U2 and corresponding rows of V1T, V2T.
   */
  printf(" Component of CS factorization of X:\n\n");
  fflush(stdout);
  nag_gen_real_mat_print(Nag_ColMajor, Nag_GeneralMatrix, Nag_NonUnitDiag, r,
                         1, theta, r, "    Theta", 0, &fail);
  if (fail.code != NE_NOERROR) {
    printf("Error from nag_gen_real_mat_print (x04cac).\n%s\n", fail.message);
    exit_status = 3;
    goto END;
  }
  printf("\n");
  fflush(stdout);

  /* And this is how you might pass partitions as separate matrices. */
  nag_zuncsd(order, Nag_AllU, Nag_AllU, Nag_AllVT, Nag_AllVT, Nag_UpperMinus,
             m, p, q, x11, pdx11, x12, pdx12, x21, pdx21, x22, pdx22, theta,
             u1, p, u2, m - p, v1t, q, v2t, m - q, &fail);
  if (fail.code != NE_NOERROR) {
    printf("Error from nag_zuncsd (f08rnc).\n%s\n", fail.message);
    exit_status = 4;
    goto END;
  }
  if (reprint != 0) {
    printf("Component of CS factorization of X using separate matrices:\n");
    fflush(stdout);
    nag_gen_real_mat_print(Nag_ColMajor, Nag_GeneralMatrix, Nag_NonUnitDiag,
                           r, 1, theta, r, "    Theta", 0, &fail);
    if (fail.code != NE_NOERROR) {
      printf("Error from nag_gen_real_mat_print (x04cac).\n%s\n",
             fail.message);
      exit_status = 5;
      goto END;
    }
    printf("\n");
    fflush(stdout);
  }

  if (recombine != 0) {
    /* Recombining should return the original matrix. 
       Assemble Sigma_p into X 
     */
    for (i = 1; i <= m; i++) {
      for (j = 1; j <= m; j++) {
        X(i, j) = czero;
      }
    }
    n11 = MIN(p, q) - r;
    n12 = MIN(p, m - q) - r;
    n21 = MIN(m - p, q) - r;
    n22 = MIN(m - p, m - q) - r;

    /* top half */
    for (j = 1; j <= n11; j++) {
      X(j, j) = cone;
    }
    for (j = 1; j <= r; j++) {
      X(j + n11, j + n11).re = cos(theta[j - 1]);
      X(j + n11, j + n11).im = 0.0;
      X(j + n11, j + n11 + r + n21 + n22).re = -sin(theta[j - 1]);
      X(j + n11, j + n11 + r + n21 + n22).im = 0.0;
    }
    for (j = 1; j <= n12; j++) {
      X(j + n11 + r, j + n11 + r + n21 + n22 + r).re = -1.0;
      X(j + n11 + r, j + n11 + r + n21 + n22 + r).im = 0.0;
    }
    /* bottom half */
    for (j = 1; j <= n22; j++) {
      X(p + j, q + j) = cone;
    }
    for (j = 1; j <= r; j++) {
      X(p + n22 + j, j + n11).re = sin(theta[j - 1]);
      X(p + n22 + j, j + n11).im = 0.0;
      X(p + n22 + j, j + r + n21 + n22).re = cos(theta[j - 1]);
      X(p + n22 + j, j + r + n21 + n22).im = 0.0;
    }
    for (j = 1; j <= n21; j++) {
      X(p + n22 + r + j, n11 + r + j) = cone;
    }

    /* multiply U * Sigma_p into w */
    nag_zgemm(order, Nag_NoTrans, Nag_NoTrans, m, m, m, cone,
              &U(1, 1), pdu, &X(1, 1), pdx, czero, &W(1, 1), pdw, &fail);
    if (fail.code != NE_NOERROR) {
      printf("Error from nag_zgemm (f16zac).\n%s\n", fail.message);
      exit_status = 6;
      goto END;
    }
    /* form U * Sigma_p * V^T into u */
    nag_zgemm(order, Nag_NoTrans, Nag_NoTrans, m, m, m, cone,
              &W(1, 1), pdw, &V(1, 1), pdv, czero, &U(1, 1), pdu, &fail);
    if (fail.code != NE_NOERROR) {
      printf("Error from nag_zgemm (f16zac).\n%s\n", fail.message);
      exit_status = 7;
      goto END;
    }
    /* nag_gen_complx_mat_print_comp (x04dbc).
     * Print least squares solutions.
     */
    nag_gen_complx_mat_print_comp(order, Nag_GeneralMatrix, Nag_NonUnitDiag,
                                  m, m, &U(1, 1), pdu, Nag_BracketForm,
                                  "%7.4f", "    U * Sigma_p * V^T",
                                  Nag_IntegerLabels, 0, Nag_IntegerLabels, 0,
                                  80, 0, 0, &fail);
    if (fail.code != NE_NOERROR) {
      printf("Error from nag_gen_complx_mat_print_comp (x04dbc).\n%s\n",
             fail.message);
      exit_status = 8;
      goto END;
    }
    printf("\n");
    fflush(stdout);
  }

END:
  NAG_FREE(x);
  NAG_FREE(u);
  NAG_FREE(v);
  NAG_FREE(w);
  NAG_FREE(theta);
  NAG_FREE(x11);
  NAG_FREE(x12);
  NAG_FREE(x21);
  NAG_FREE(x22);
  NAG_FREE(u1);
  NAG_FREE(u2);
  NAG_FREE(v1t);
  NAG_FREE(v2t);
  return exit_status;
}