//Ｒ用 関数
// 

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

#include <R.h>
#include <Rdefines.h>
#include <R_ext/Parse.h>
#include <R_ext/Applic.h>
#include <R_ext/Lapack.h>


const int gl_incx=1, gl_incy=1;
const double one = 1.0;
const double minusOne = -1.0;
const double zero = 0.0;

const int intZero = 0L;

int gl_lda;






/*
  tol is used to determine the effective rank of x.
  if tol < 0.0 , machine eps is used for tol.
 */
void dqrls2(double *x, int *n, int *p, double *y, int *ny, double *tol,
           double *b, double *rsd, double *qty, int *k, int *jpvt, double *qraux)
{
  double work0;
  double *work;
  double *x1;
  int nny = (*n)*(*ny);
  int np = (*n)*(*p);
  int lwork0, lwork;
  int info_dgeqp3;
  int info_dqrsl, job = 1110;
  int info_dgelsy;
  int ii, jj, kk;
  
  x1 = (double *) R_alloc(np, sizeof(double));
  // copy x to x1
  F77_CALL(dcopy)(&np, x, &gl_incx, x1, &gl_incy);
  // copy y to rsd
  F77_CALL(dcopy)(&nny, y, &gl_incx, rsd, &gl_incy);
  // calc LWORK
  lwork0 = -1;
  F77_CALL(dgelsy)(n, p, ny, x, n, y, n, jpvt, tol, k, &work0, &lwork0, &info_dgelsy);
  //Rprintf("work0 : %f\n", work0);
  //1Rprintf("n : %d\n", *n);
  // exec LM
  lwork = work0;
  work = (double *) R_alloc(lwork, sizeof(double));
  F77_CALL(dgelsy)(n, p, ny, x, n, y, n, jpvt, tol, k, work, &lwork, &info_dgelsy);
  for( ii=0; ii<*p; ii++ )
    for( jj=0; jj<*ny; jj++ )
      b[ii+jj*(*p)] = y[ii+jj*(*n)];
  // calc rsd
  F77_CALL(dgemm)("N", "N", n, ny, p, &minusOne, x1, n, b, p, &one, rsd, n);
}

void dqrls3(double *x, int *n, int *p, double *y, int *ny, double *tol,
           double *b, double *rsd, double *qty, int *k, int *jpvt, double *qraux)
{
  double work0;
  double *work;
  double *x1;
  int lwork0, lwork;
  int INFO;
  int info_dgeqp3;
  int info_dqrsl, job = 1110;
  int info_dgelsy;
  int ii, jj, kk;
  int nny = (*n)*(*ny);
  int np = (*n)*(*p);
  
  x1 = (double *) R_alloc(np, sizeof(double));
  // copy x to x1
  F77_CALL(dcopy)(&np, x, &gl_incx, x1, &gl_incy);
  // copy y to rsd
  F77_CALL(dcopy)(&nny, y, &gl_incx, rsd, &gl_incy);
  // copy y to qty
  F77_CALL(dcopy)(&nny, y, &gl_incx, qty, &gl_incy);
  
  //////////////////////////////////////////////////
  // calc LWORK
  lwork0 = -1;
  F77_CALL(dgelsy)(n, p, ny, x, n, y, n, jpvt, tol, k, &work0, &lwork0, &info_dgelsy);
  //Rprintf("work0 : %f\n", work0);
  //Rprintf("n : %d\n", *n);
  lwork = work0;
  work = (double *) R_alloc(lwork, sizeof(double));
  //Rprintf("lwork : %d\n", lwork);
  
  const int IMAX=1, IMIN=2;
  int I, IASCL, IBSCL, ISMAX, ISMIN, J, LWKMIN, LWKOPT, MN, NB, NB1, NB2, NB3, NB4;
  double ANRM, BIGNUM, BNRM, C1, C2, S1, S2, SMAX, SMAXPR, SMIN, SMINPR, SMLNUM, WSIZE;
  MN = (*n)<(*p)?(*n):(*p);
  ISMIN = MN + 1;
  ISMAX = 2*MN + 1;
  //////////////////////////////////////////////////
  // Get machine parameters
  SMLNUM = F77_CALL(dlamch)( "S" ) / F77_CALL(dlamch)( "P" );
  BIGNUM = 1.0 / SMLNUM;
  F77_CALL(dlabad)( &SMLNUM, &BIGNUM );
  //////////////////////////////////////////////////
  // Scale A, B if max entries outside range [SMLNUM,BIGNUM]
  ANRM = F77_CALL(dlange)( "M", n, p, x, n, &work0 );
  /*
    WORK    (workspace) DOUBLE PRECISION array, dimension (MAX(1,LWORK)),
            where LWORK >= M when NORM = 'I'; otherwise, WORK is not
            referenced.
   */
  IASCL = 0;
  if( ANRM>0.0 & ANRM<SMLNUM ){
    // Scale matrix norm up to SMLNUM
    F77_CALL(dlascl)( "G", &intZero, &intZero, &ANRM, &SMLNUM, n, p, x, n, &INFO );
    IASCL = 1;
  }
  else if( ANRM>BIGNUM ){
    // Scale matrix norm down to BIGNUM
    F77_CALL(dlascl)( "G", &intZero, &intZero, &ANRM, &BIGNUM, n, p, x, n, &INFO );
    IASCL = 2;
  }
  else if( ANRM==0 ){
    // Matrix all zero. Return zero solution.
    int MAX_MN = (*n)<(*p)?(*p):(*n);
    F77_CALL(dlaset)( "F", &MAX_MN, ny, &zero, &zero, y, n );
    *k = 0;
    return;
  }
  BNRM = F77_CALL(dlange)( "M", n, ny, y, n, &work0 );
  IBSCL = 0;
  if( BNRM>0.0 & BNRM<SMLNUM ){
    // Scale matrix norm up to SMLNUM
    F77_CALL(dlascl)( "G", &intZero, &intZero, &BNRM, &SMLNUM, n, ny, y, n, &INFO );
    IBSCL = 1;
  }
  else if( BNRM>BIGNUM ){
    // Scale matrix norm down to BIGNUM
    F77_CALL(dlascl)( "G", &intZero, &intZero, &BNRM, &BIGNUM, n, ny, y, n, &INFO );
    IBSCL = 2;
  }
  //////////////////////////////////////////////////
  // Compute QR factorization with column pivoting of A:
  lwork0 = lwork - MN;
  /*
  Rprintf("lwork0 : %d\n", lwork0);
  Rprintf("n : %d\n", *n);
  Rprintf("p : %d\n", *p);
  Rprintf("MN : %d\n", MN);
  for( ii=0; ii<(*n)*(*p); ii++ ){
    Rprintf("x[ii] : %e\n", x[ii]);
  }
    Rprintf("x[0+(*n)*(2-1)] : %e\n", x[0+(*n)*(2-1)]);
    Rprintf("x[(2-1)+(*n)*(2-1)] : %e\n", x[(2-1)+(*n)*(2-1)]);
   */
  //double ww[10];
  //F77_CALL(dgeqp3)(n, p, x, n, jpvt, qraux, ww, &lwork0, &info_dgeqp3);
  F77_CALL(dgeqp3)(n, p, x, n, jpvt, qraux, &work[ MN+1-1 ], &lwork0, &info_dgeqp3);
  
  //  Rprintf("info_dgeqp3 : %d\n", info_dgeqp3);
  //  Rprintf("x[0+(*n)*(2-1)] : %e\n", x[0+(*n)*(2-1)]);
  //  Rprintf("x[(2-1)+(*n)*(2-1)] : %e\n", x[(2-1)+(*n)*(2-1)]);
  //for( ii=0; ii<(*n)*(*p); ii++ ){
  //  Rprintf("x[ii] : %e\n", x[ii]);
  //}
    // Details of Householder rotations stored in WORK.
  F77_CALL(dcopy)(&MN, qraux, &gl_incx, work, &gl_incy);
  //////////////////////////////////////////////////
  // Determine RANK using incremental condition estimation
  work[ ISMIN-1 ] = 1.0;
  work[ ISMAX-1 ] = 1.0;
  SMAX = fabs( x[ 0 ] );
  SMIN = SMAX;
  if( fabs( x[0] )==0.0 ){
    int MAX_MN = (*n)<(*p)?(*p):(*n);
    F77_CALL(dlaset)( "F", &MAX_MN, ny, &zero, &zero, y, n );
    *k = 0;
    return;
  }
  else{
    *k = 1;
  }
  while( (*k)<MN ){
    //Rprintf("rank : %d\n", (*k));
    I = *k + 1;
    /*
    Rprintf("IMIN : %d\n", IMIN);
    Rprintf("IMAX : %d\n", IMAX);
    Rprintf("SMIN : %e\n", SMIN);
    Rprintf("SMAX : %e\n", SMAX);
    Rprintf("work[ ISMIN-1 ] : %e\n", work[ ISMIN-1 ]);
    Rprintf("x[0+(*n)*(I-1)] : %e\n", x[0+(*n)*(I-1)]);
    Rprintf("x[(I-1)+(*n)*(I-1)] : %e\n", x[(I-1)+(*n)*(I-1)]);
     */
    F77_CALL(dlaic1)( &IMIN, k, &work[ ISMIN-1 ], &SMIN, &x[0+(*n)*(I-1)],
                      &x[(I-1)+(*n)*(I-1)], &SMINPR, &S1, &C1 );
    F77_CALL(dlaic1)( &IMAX, k, &work[ ISMAX-1 ], &SMAX, &x[0+(*n)*(I-1)],
                      &x[(I-1)+(*n)*(I-1)], &SMAXPR, &S2, &C2 );
    //Rprintf("tol : %e\n", *tol);
    //Rprintf("SMINPR : %e\n", SMINPR);
    //Rprintf("SMAXPR : %e\n", SMAXPR);
    if( SMAXPR*(*tol)<=SMINPR ){
      for( I=0; I<(*k); I++ ){
        work[ ISMIN+I-1 ] = S1*work[ ISMIN+I-1 ];
        work[ ISMAX+I-1 ] = S2*work[ ISMAX+I-1 ];
      }
      work[ISMIN+(*k)-1] = C1;
      work[ISMAX+(*k)-1] = C2;
      SMIN = SMINPR;
      SMAX = SMAXPR;
      *k = *k + 1;
      continue;
    }
    break;
  }
  if( (*k) < (*p) ){
    lwork0 = lwork - 2*MN;
    F77_CALL(dtzrzf)( k, p, x, n, &work[ MN+1-1 ], &work[ 2*MN+1-1 ], &lwork0, &INFO );
  }
  //////////////////////////////////////////////////
  // B(1:M,1:NRHS) := Q' * B(1:M,1:NRHS)
  lwork0 = lwork - 2*MN;
  F77_CALL(dormqr)( "Left", "Transpose", n, ny, &MN, x, n, work,
                    y, n, &work[ 2*MN+1-1 ], &lwork0, &INFO );
  // qty
  //F77_CALL(dormqr)( "Left", "Transpose", n, ny, &MN, x, n, work,
  //                  qty, n, &work[ 2*MN+1-1 ], &lwork0, &INFO );
  F77_CALL(dcopy)(&nny, y, &gl_incx, qty, &gl_incy);
  /*
  int ju = ((*k)<(*n-1))?(*k):(*n-1);
  double temp, t;
  for( jj=0; jj<ju; jj++ ){
    if( qraux[jj] != 0.0 ){
      temp = x[jj+jj*(*n)]; //(j,j)
      x[jj+jj*(*n)] = qraux[jj];
      ii = *n-jj+1 -1;
      t = - F77_CALL(ddot)(&ii, &x[jj+jj*(*n)], &gl_incx, &qty[jj], &gl_incy)/x[jj+jj*(*n)];
      F77_CALL(daxpy)(&ii, &t, &x[jj+jj*(*n)], &gl_incx, &qty[jj], &gl_incy);
      x[jj+jj*(*n)] = temp;
    }
  }*/
  //            do 90 j = 1, ju
  //             if (qraux(j) .eq. 0.0d0) go to 80
  //                temp = x(j,j)
  //                x(j,j) = qraux(j)
  //                t = -ddot(n-j+1,x(j,j),1,qty(j),1)/x(j,j)
  //                call daxpy(n-j+1,t,x(j,j),1,qty(j),1)
  //                x(j,j) = temp
  // 80          continue
  // 90       continue
  //////////////////////////////////////////////////
  // B(1:RANK,1:NRHS) := inv(T11) * B(1:RANK,1:NRHS)
  F77_CALL(dtrsm)( "Left", "Upper", "No transpose", "Non-unit", k, ny, &one, x, n, y, n );
  for( jj=0; jj<(*ny); jj++ ){
    for( ii=(*k); ii<(*p); ii++ ){
      //Rprintf("ii : %d\n", ii);
      //Rprintf("jj : %d\n", jj);
      //Rprintf("y[ii+jj*(*n)] : %f\n", y[ii+jj*(*n)]);
      y[ii+jj*(*n)] = 0.0;
      //Rprintf("y[ii+jj*(*n)] : %f\n", y[ii+jj*(*n)]);
    }
  }
  //////////////////////////////////////////////////
  // B(1:N,1:NRHS) := Y' * B(1:N,1:NRHS)
  if( (*k)<(*p) ){
    //Rprintf("(*k)<(*p)\n");
    //Rprintf("rank : %d\n", *k);
    //Rprintf("p : %d\n", *p);
    int n_rank = *p - *k;
    //Rprintf("n - rank : %d\n", n_rank);
    lwork0 = lwork - 2*MN;
    F77_CALL(dormrz)( "Left", "Transpose", p, ny, k, &n_rank, x, n,
                      &work[ MN+1-1 ], y, n, &work[ 2*MN+1-1 ], &lwork0, &INFO );
  }
  //////////////////////////////////////////////////
  // B(1:N,1:NRHS) := P * B(1:N,1:NRHS)
  for( jj=0; jj<(*ny); jj++ ){
    for( ii=0; ii<(*p); ii++ ){
      work[ jpvt[ ii ]-1 ] = y[ii+jj*(*n)];
    }
    F77_CALL(dcopy)( p, work, &gl_incx, &y[0+jj*(*n)], &gl_incy );
  }
  //////////////////////////////////////////////////
  // Undo scaling
  if( IASCL==1 ){
    F77_CALL(dlascl)( "G", &intZero, &intZero, &ANRM, &SMLNUM, p, ny, y, n, &INFO );
    F77_CALL(dlascl)( "U", &intZero, &intZero, &SMLNUM, &ANRM, k, k, x, n, &INFO );
  }
  else if( IASCL==2 ){
    F77_CALL(dlascl)( "G", &intZero, &intZero, &ANRM, &BIGNUM, p, ny, y, n, &INFO );
    F77_CALL(dlascl)( "U", &intZero, &intZero, &BIGNUM, &ANRM, k, k, x, n, &INFO );
  }
  if( IBSCL==1 ){
    F77_CALL(dlascl)( "G", &intZero, &intZero, &SMLNUM, &BNRM, p, ny, y, n, &INFO );
  }
  else if( IBSCL==2 ){
    F77_CALL(dlascl)( "G", &intZero, &intZero, &BIGNUM, &BNRM, p, ny, y, n, &INFO );
  }
  //////////////////////////////////////////////////
  // copy coef
  //for( ii=0; ii<*p; ii++ )
  //  for( jj=0; jj<*ny; jj++ )
  //    b[ii+jj*(*p)] = y[ii+jj*(*n)];
  for( jj=0; jj<*ny; jj++ )
    F77_CALL(dcopy)(k, &y[jj*(*n)], &gl_incx, &b[jj*(*p)], &gl_incy);
  //////////////////////////////////////////////////
  // calc rsd
  F77_CALL(dgemm)("N", "N", n, ny, p, &minusOne, x1, n, b, p, &one, rsd, n);
}

void dqrls4(double *x, int *n, int *p, double *y, int *ny, double *tol,
           double *b, double *rsd, double *qty, int *k, int *jpvt, double *qraux)
{
  double work0;
  double *work;
  double *x1;
  int lwork0, lwork;
  int INFO;
  int info_dgeqp3;
  int info_dqrsl, job = 1110;
  int info_dgelsy;
  int ii, jj, kk;
  int nny = (*n)*(*ny);
  int np = (*n)*(*p);
  
  x1 = (double *) R_alloc(np, sizeof(double));
  // copy x to x1
  F77_CALL(dcopy)(&np, x, &gl_incx, x1, &gl_incy);
  // copy y to rsd
  F77_CALL(dcopy)(&nny, y, &gl_incx, rsd, &gl_incy);
  // copy y to qty
  F77_CALL(dcopy)(&nny, y, &gl_incx, qty, &gl_incy);
  
  //////////////////////////////////////////////////
  // calc LWORK
  lwork0 = -1;
  F77_CALL(dgelsy)(n, p, ny, x, n, y, n, jpvt, tol, k, &work0, &lwork0, &info_dgelsy);
  lwork = work0;
  work = (double *) R_alloc(lwork, sizeof(double));
  
  const int IMAX=1, IMIN=2;
  int I, IASCL, IBSCL, ISMAX, ISMIN, J, LWKMIN, LWKOPT, MN, NB, NB1, NB2, NB3, NB4;
  double ANRM, BIGNUM, BNRM, C1, C2, S1, S2, SMAX, SMAXPR, SMIN, SMINPR, SMLNUM, WSIZE;
  MN = (*n)<(*p)?(*n):(*p);
  ISMIN = MN + 1;
  ISMAX = 2*MN + 1;
  //////////////////////////////////////////////////
  // Get machine parameters
  SMLNUM = F77_CALL(dlamch)( "S" ) / F77_CALL(dlamch)( "P" );
  BIGNUM = 1.0 / SMLNUM;
  F77_CALL(dlabad)( &SMLNUM, &BIGNUM );
  //////////////////////////////////////////////////
  // Scale A, B if max entries outside range [SMLNUM,BIGNUM]
  ANRM = F77_CALL(dlange)( "M", n, p, x, n, &work0 );
  /*
    WORK    (workspace) DOUBLE PRECISION array, dimension (MAX(1,LWORK)),
            where LWORK >= M when NORM = 'I'; otherwise, WORK is not
            referenced.
   */
  IASCL = 0;
  if( ANRM>0.0 & ANRM<SMLNUM ){
    // Scale matrix norm up to SMLNUM
    F77_CALL(dlascl)( "G", &intZero, &intZero, &ANRM, &SMLNUM, n, p, x, n, &INFO );
    IASCL = 1;
  }
  else if( ANRM>BIGNUM ){
    // Scale matrix norm down to BIGNUM
    F77_CALL(dlascl)( "G", &intZero, &intZero, &ANRM, &BIGNUM, n, p, x, n, &INFO );
    IASCL = 2;
  }
  else if( ANRM==0 ){
    // Matrix all zero. Return zero solution.
    int MAX_MN = (*n)<(*p)?(*p):(*n);
    F77_CALL(dlaset)( "F", &MAX_MN, ny, &zero, &zero, y, n );
    *k = 0;
    return;
  }
  BNRM = F77_CALL(dlange)( "M", n, ny, y, n, &work0 );
  IBSCL = 0;
  if( BNRM>0.0 & BNRM<SMLNUM ){
    // Scale matrix norm up to SMLNUM
    F77_CALL(dlascl)( "G", &intZero, &intZero, &BNRM, &SMLNUM, n, ny, y, n, &INFO );
    IBSCL = 1;
  }
  else if( BNRM>BIGNUM ){
    // Scale matrix norm down to BIGNUM
    F77_CALL(dlascl)( "G", &intZero, &intZero, &BNRM, &BIGNUM, n, ny, y, n, &INFO );
    IBSCL = 2;
  }
  //////////////////////////////////////////////////
  // Compute QR factorization with column pivoting of A:
  lwork0 = lwork - MN;
  F77_CALL(dgeqp3)(n, p, x, n, jpvt, qraux, &work[ MN+1-1 ], &lwork0, &info_dgeqp3);
  //F77_CALL(dgeqrf)(n, p, x, n, qraux, &work[ MN+1-1 ], &lwork0, &info_dgeqp3);
  F77_CALL(dcopy)(&MN, qraux, &gl_incx, work, &gl_incy);
  //////////////////////////////////////////////////
  // Determine RANK using incremental condition estimation
  work[ ISMIN-1 ] = 1.0;
  work[ ISMAX-1 ] = 1.0;
  SMAX = fabs( x[ 0 ] );
  SMIN = SMAX;
  if( fabs( x[0] )==0.0 ){
    int MAX_MN = (*n)<(*p)?(*p):(*n);
    F77_CALL(dlaset)( "F", &MAX_MN, ny, &zero, &zero, y, n );
    *k = 0;
    return;
  }
  else{
    *k = 1;
  }
  while( (*k)<MN ){
    I = *k + 1;
    F77_CALL(dlaic1)( &IMIN, k, &work[ ISMIN-1 ], &SMIN, &x[0+(*n)*(I-1)],
                      &x[(I-1)+(*n)*(I-1)], &SMINPR, &S1, &C1 );
    F77_CALL(dlaic1)( &IMAX, k, &work[ ISMAX-1 ], &SMAX, &x[0+(*n)*(I-1)],
                      &x[(I-1)+(*n)*(I-1)], &SMAXPR, &S2, &C2 );
    if( SMAXPR*(*tol)<=SMINPR ){
      for( I=0; I<(*k); I++ ){
        work[ ISMIN+I-1 ] = S1*work[ ISMIN+I-1 ];
        work[ ISMAX+I-1 ] = S2*work[ ISMAX+I-1 ];
      }
      work[ISMIN+(*k)-1] = C1;
      work[ISMAX+(*k)-1] = C2;
      SMIN = SMINPR;
      SMAX = SMAXPR;
      *k = *k + 1;
      continue;
    }
    break;
  }
  int MN2 = ((*k)<MN)?(*k):(MN);
  //////////////////////////////////////////////////
  // B(1:M,1:NRHS) := Q' * B(1:M,1:NRHS)
  lwork0 = lwork - 2*MN;
  F77_CALL(dormqr)( "Left", "Transpose", n, ny, &MN2, x, n, work,
                    y, n, &work[ 2*MN+1-1 ], &lwork0, &INFO );
  // qty
  F77_CALL(dcopy)(&nny, y, &gl_incx, qty, &gl_incy);
  //////////////////////////////////////////////////
  // B(1:RANK,1:NRHS) := inv(T11) * B(1:RANK,1:NRHS)
  F77_CALL(dtrsm)( "Left", "Upper", "No transpose", "Non-unit", k, ny, &one, x, n, y, n );
  for( jj=0; jj<(*ny); jj++ ){
    for( ii=(*k); ii<(*p); ii++ ){
      y[ii+jj*(*n)] = 0.0;
    }
  }
  //////////////////////////////////////////////////
  // B(1:N,1:NRHS) := P * B(1:N,1:NRHS)
  //for( jj=0; jj<(*ny); jj++ ){
  //  for( ii=0; ii<(*p); ii++ ){
  //    work[ jpvt[ ii ]-1 ] = y[ii+jj*(*n)];
  //  }
  //  F77_CALL(dcopy)( p, work, &gl_incx, &y[0+jj*(*n)], &gl_incy );
  //}
  //////////////////////////////////////////////////
  // Undo scaling
  if( IASCL==1 ){
    F77_CALL(dlascl)( "G", &intZero, &intZero, &ANRM, &SMLNUM, p, ny, y, n, &INFO );
    F77_CALL(dlascl)( "U", &intZero, &intZero, &SMLNUM, &ANRM, k, k, x, n, &INFO );
  }
  else if( IASCL==2 ){
    F77_CALL(dlascl)( "G", &intZero, &intZero, &ANRM, &BIGNUM, p, ny, y, n, &INFO );
    F77_CALL(dlascl)( "U", &intZero, &intZero, &BIGNUM, &ANRM, k, k, x, n, &INFO );
  }
  if( IBSCL==1 ){
    F77_CALL(dlascl)( "G", &intZero, &intZero, &SMLNUM, &BNRM, p, ny, y, n, &INFO );
  }
  else if( IBSCL==2 ){
    F77_CALL(dlascl)( "G", &intZero, &intZero, &BIGNUM, &BNRM, p, ny, y, n, &INFO );
  }
  //////////////////////////////////////////////////
  // copy coef
  for( jj=0; jj<*ny; jj++ )
    F77_CALL(dcopy)(k, &y[jj*(*n)], &gl_incx, &b[jj*(*p)], &gl_incy);
  //////////////////////////////////////////////////
  // calc rsd
  F77_CALL(dgemm)("N", "N", n, ny, k, &minusOne, x1, n, b, k, &one, rsd, n);
}