/* nag_zuncsd (f08rnc) Example Program.
 *
 * Copyright 2014 Numerical Algorithms Group.
 *
 * Mark 24, 2013.
 */

#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("%ld%ld%ld%*[^\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 orthogonal 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;
}