// test: min || B - A.T*X ||
func TestLeastSquaresLQ(t *testing.T) {
	M := 723
	N := 811
	K := 273
	nb := 32
	conf := gomas.NewConf()
	conf.LB = nb

	tau := cmat.NewMatrix(M, 1)
	A := cmat.NewMatrix(M, N)
	src := cmat.NewFloatNormSource()
	A.SetFrom(src)
	B0 := cmat.NewMatrix(M, K)
	B0.SetFrom(src)
	B := cmat.NewMatrix(N, K)

	// B = A.T*B0
	blasd.Mult(B, A, B0, 1.0, 0.0, gomas.TRANSA, conf)

	W := lapackd.Workspace(lapackd.LQFactorWork(A, conf))
	lapackd.LQFactor(A, tau, W, conf)

	// B' = A.-1*B
	lapackd.LQSolve(B, A, tau, W, gomas.TRANS, conf)

	// expect B[0:M,0:K] == B0[0:M,0:K], B[M:N,0:K] == 0
	var X cmat.FloatMatrix

	X.SubMatrix(B, 0, 0, M, K)
	blasd.Plus(&X, B0, 1.0, -1.0, gomas.NONE)
	nrm := lapackd.NormP(&X, lapackd.NORM_ONE)

	t.Logf("M=%d, N=%d  ||B0 - min( ||A.T*X - B0|| ) ||_1: %e\n", M, N, nrm)
}

func MVMult(Y, A, X *cmat.FloatMatrix, alpha, beta float64, bits int, confs ...*gomas.Config) *gomas.Error {
	ok := true
	yr, yc := Y.Size()
	ar, ac := A.Size()
	xr, xc := X.Size()

	if ar*ac == 0 {
		return nil
	}
	if yr != 1 && yc != 1 {
		return gomas.NewError(gomas.ENEED_VECTOR, "MVMult")
	}
	if xr != 1 && xc != 1 {
		return gomas.NewError(gomas.ENEED_VECTOR, "MVMult")
	}
	nx := X.Len()
	ny := Y.Len()

	if bits&gomas.TRANSA != 0 {
		bits |= gomas.TRANS
	}
	if bits&gomas.TRANS != 0 {
		ok = ny == ac && nx == ar
	} else {
		ok = ny == ar && nx == ac
	}
	if !ok {
		return gomas.NewError(gomas.ESIZE, "MVMult")
	}
	if beta != 1.0 {
		vscal(Y, beta, ny)
	}
	gemv(Y, A, X, alpha, beta, bits, 0, nx, 0, ny)
	return nil
}

/*
 * Reduce a general M-by-N matrix A to upper or lower bidiagonal form B
 * by an ortogonal transformation A = Q*B*P.T,  B = Q.T*A*P
 *
 *
 * Arguments
 *   A     On entry, the real M-by-N matrix. On exit the upper/lower
 *         bidiagonal matrix and ortogonal matrices Q and P.
 *
 *   tauq  Scalar factors for elementary reflector forming the
 *         ortogonal matrix Q.
 *
 *   taup  Scalar factors for elementary reflector forming the
 *         ortogonal matrix P.
 *
 *   W     Workspace needed for reduction.
 *
 *   conf  Current blocking configuration. Optional.
 *
 *
 * Details
 *
 * Matrices Q and P are products of elementary reflectors H(k) and G(k)
 *
 * If M > N:
 *     Q = H(1)*H(2)*...*H(N)   and P = G(1)*G(2)*...*G(N-1)
 *
 * where H(k) = 1 - tauq*u*u.T and G(k) = 1 - taup*v*v.T
 *
 * Elementary reflector H(k) are stored on columns of A below the diagonal with
 * implicit unit value on diagonal entry. Vector TAUQ holds corresponding scalar
 * factors. Reflector G(k) are stored on rows of A right of first superdiagonal
 * with implicit unit value on superdiagonal. Corresponding scalar factors are
 * stored on vector TAUP.
 *
 * If M < N:
 *   Q = H(1)*H(2)*...*H(N-1)   and P = G(1)*G(2)*...*G(N)
 *
 * where H(k) = 1 - tauq*u*u.T and G(k) = 1 - taup*v*v.T
 *
 * Elementary reflector H(k) are stored on columns of A below the first sub diagonal
 * with implicit unit value on sub diagonal entry. Vector TAUQ holds corresponding
 * scalar factors. Reflector G(k) are sotre on rows of A right of diagonal with
 * implicit unit value on superdiagonal. Corresponding scalar factors are stored
 * on vector TAUP.
 *
 * Contents of matrix A after reductions are as follows.
 *
 *    M = 6 and N = 5:                  M = 5 and N = 6:
 *
 *    (  d   e   v1  v1  v1 )           (  d   v1  v1  v1  v1  v1 )
 *    (  u1  d   e   v2  v2 )           (  e   d   v2  v2  v2  v2 )
 *    (  u1  u2  d   e   v3 )           (  u1  e   d   v3  v3  v3 )
 *    (  u1  u2  u3  d   e  )           (  u1  u2  e   d   v4  v4 )
 *    (  u1  u2  u3  u4  d  )           (  u1  u2  u3  e   d   v5 )
 *    (  u1  u2  u3  u4  u5 )
 */
func BDReduce(A, tauq, taup, W *cmat.FloatMatrix, confs ...*gomas.Config) *gomas.Error {
	var err *gomas.Error = nil
	conf := gomas.CurrentConf(confs...)
	_ = conf

	wmin := wsBired(A, 0)
	wsz := W.Len()
	if wsz < wmin {
		return gomas.NewError(gomas.EWORK, "ReduceBidiag", wmin)
	}
	lb := conf.LB
	wneed := wsBired(A, lb)
	if wneed > wsz {
		lb = estimateLB(A, wsz, wsBired)
	}
	if m(A) >= n(A) {
		if lb > 0 && n(A) > lb {
			blkBidiagLeft(A, tauq, taup, W, lb, conf)
		} else {
			unblkReduceBidiagLeft(A, tauq, taup, W)
		}
	} else {
		if lb > 0 && m(A) > lb {
			blkBidiagRight(A, tauq, taup, W, lb, conf)
		} else {
			unblkReduceBidiagRight(A, tauq, taup, W)
		}
	}
	return err
}

/*
 * Solve a system of linear equations A*X = B or A.T*X = B with general N-by-N
 * matrix A using the LU factorization computed by LUFactor().
 *
 * Arguments:
 *  B      On entry, the right hand side matrix B. On exit, the solution matrix X.
 *
 *  A      The factor L and U from the factorization A = P*L*U as computed by
 *         LUFactor()
 *
 *  pivots The pivot indices from LUFactor().
 *
 *  flags  The indicator of the form of the system of equations.
 *         If flags&TRANSA then system is transposed. All other values
 *         indicate non transposed system.
 *
 * Compatible with lapack.DGETRS.
 */
func LUSolve(B, A *cmat.FloatMatrix, pivots Pivots, flags int, confs ...*gomas.Config) *gomas.Error {
	var err *gomas.Error = nil
	conf := gomas.DefaultConf()
	if len(confs) > 0 {
		conf = confs[0]
	}
	ar, ac := A.Size()
	br, _ := B.Size()
	if ar != ac {
		return gomas.NewError(gomas.ENOTSQUARE, "SolveLU")
	}
	if br != ac {
		return gomas.NewError(gomas.ESIZE, "SolveLU")
	}
	if pivots != nil {
		applyPivots(B, pivots)
	}
	if flags&gomas.TRANSA != 0 {
		// transposed X = A.-1*B == (L.T*U.T).-1*B == U.-T*(L.-T*B)
		blasd.SolveTrm(B, A, 1.0, gomas.LOWER|gomas.UNIT|gomas.TRANSA, conf)
		blasd.SolveTrm(B, A, 1.0, gomas.UPPER|gomas.TRANSA, conf)
	} else {
		// non-transposed X = A.-1*B == (L*U).-1*B == U.-1*(L.-1*B)
		blasd.SolveTrm(B, A, 1.0, gomas.LOWER|gomas.UNIT, conf)
		blasd.SolveTrm(B, A, 1.0, gomas.UPPER, conf)
	}

	return err
}

/*
 * Applies a real elementary reflector H to a real m by n matrix A,
 * from either the left or the right. H is represented in the form
 *
 *       H = I - tau * ( 1 ) * ( 1 v.T )
 *                     ( v )
 *
 * where tau is a real scalar and v is a real vector.
 *
 * If tau = 0, then H is taken to be the unit cmat.
 *
 * A is /a1\   a1 := a1 - w1
 *      \A2/   A2 := A2 - v*w1
 *             w1 := tau*(a1 + A2.T*v) if side == LEFT
 *                := tau*(a1 + A2*v)   if side == RIGHT
 *
 * Intermediate work space w1 required as parameter, no allocation.
 */
func applyHouseholder2x1(tau, v, a1, A2, w1 *cmat.FloatMatrix, flags int) *gomas.Error {
	var err *gomas.Error = nil
	tval := tau.Get(0, 0)
	if tval == 0.0 {
		return err
	}

	// shape oblivious vector copy.
	blasd.Axpby(w1, a1, 1.0, 0.0)
	if flags&gomas.LEFT != 0 {
		// w1 = a1 + A2.T*v
		err = blasd.MVMult(w1, A2, v, 1.0, 1.0, gomas.TRANSA)
	} else {
		// w1 = a1 + A2*v
		err = blasd.MVMult(w1, A2, v, 1.0, 1.0, gomas.NONE)
	}
	// w1 = tau*w1
	blasd.Scale(w1, tval)

	// a1 = a1 - w1
	blasd.Axpy(a1, w1, -1.0)

	// A2 = A2 - v*w1
	if flags&gomas.LEFT != 0 {
		err = blasd.MVUpdate(A2, v, w1, -1.0)
	} else {
		err = blasd.MVUpdate(A2, w1, v, -1.0)
	}
	return err
}

// Simple and slow LQ decomposition with Givens rotations
func TestGivensLQ(t *testing.T) {
	var d cmat.FloatMatrix
	M := 149
	N := 167
	A := cmat.NewMatrix(M, N)
	A1 := cmat.NewCopy(A)

	ones := cmat.NewFloatConstSource(1.0)
	src := cmat.NewFloatNormSource()
	A.SetFrom(src)
	A0 := cmat.NewCopy(A)

	Qt := cmat.NewMatrix(N, N)
	d.Diag(Qt)
	d.SetFrom(ones)

	// R = G(n)...G(2)G(1)*A; Q = G(1).T*G(2).T...G(n).T ;  Q.T = G(n)...G(2)G(1)
	for i := 0; i < M; i++ {
		// zero elements right of diagonal
		for j := N - 2; j >= i; j-- {
			c, s, r := lapackd.ComputeGivens(A.Get(i, j), A.Get(i, j+1))
			A.Set(i, j, r)
			A.Set(i, j+1, 0.0)
			// apply rotation to this column starting from row i+1
			lapackd.ApplyGivensRight(A, j, j+1, i+1, M-i-1, c, s)
			// update Qt = G(k)*Qt
			lapackd.ApplyGivensRight(Qt, j, j+1, 0, N, c, s)
		}
	}
	// A = L*Q
	blasd.Mult(A1, A, Qt, 1.0, 0.0, gomas.TRANSB)
	blasd.Plus(A0, A1, 1.0, -1.0, gomas.NONE)
	nrm := lapackd.NormP(A0, lapackd.NORM_ONE)
	t.Logf("M=%d, N=%d ||A - L*G(1)..G(n)||_1: %e\n", M, N, nrm)
}

func TestSubMatrixGob(t *testing.T) {
	var B, As cmat.FloatMatrix
	var network bytes.Buffer
	N := 32
	A := cmat.NewMatrix(N, N)
	zeromean := cmat.NewFloatNormSource()
	A.SetFrom(zeromean)
	As.SubMatrix(A, 3, 3, N-6, N-6)

	enc := gob.NewEncoder(&network)
	dec := gob.NewDecoder(&network)

	// encode to network
	err := enc.Encode(&As)
	if err != nil {
		t.Logf("encode error: %v\n", err)
		t.FailNow()
	}

	// decode from network
	err = dec.Decode(&B)
	if err != nil {
		t.Logf("decode error: %v\n", err)
		t.FailNow()
	}

	ar, ac := As.Size()
	br, bc := B.Size()
	t.Logf("As[%d,%d] == B[%d,%d]: %v\n", ar, ac, br, bc, B.AllClose(&As))
}

/*
 * Symmetric matrix-vector multiplication. Y = beta*Y + alpha*A*X
 */
func MVMultSym(Y, A, X *cmat.FloatMatrix, alpha, beta float64, bits int, confs ...*gomas.Config) *gomas.Error {
	ok := true
	yr, yc := Y.Size()
	ar, ac := A.Size()
	xr, xc := X.Size()

	if ar*ac == 0 {
		return nil
	}
	if yr != 1 && yc != 1 {
		return gomas.NewError(gomas.ENEED_VECTOR, "MVMultSym")
	}
	if xr != 1 && xc != 1 {
		return gomas.NewError(gomas.ENEED_VECTOR, "MVMultSym")
	}
	nx := X.Len()
	ny := Y.Len()

	ok = ny == ar && nx == ac && ac == ar
	if !ok {
		return gomas.NewError(gomas.ESIZE, "MVMultSym")
	}
	if beta != 1.0 {
		vscal(Y, beta, ny)
	}
	symv(Y, A, X, alpha, bits, nx)
	return nil
}

func TestJSON(t *testing.T) {
	var B cmat.FloatMatrix
	var network bytes.Buffer
	N := 26
	A := cmat.NewMatrix(N, N)
	zeromean := cmat.NewFloatNormSource()
	A.SetFrom(zeromean)

	enc := json.NewEncoder(&network)
	dec := json.NewDecoder(&network)

	// encode to network
	err := enc.Encode(A)
	//t.Logf("bytes: %v\n", string(network.Bytes()))
	if err != nil {
		t.Logf("encode error: %v\n", err)
		t.FailNow()
	}

	// decode from network
	err = dec.Decode(&B)
	if err != nil {
		t.Logf("decode error: %v\n", err)
		t.FailNow()
	}
	t.Logf("A == B: %v\n", B.AllClose(A))
}

func TestPartition2D(t *testing.T) {
	var ATL, ATR, ABL, ABR, As cmat.FloatMatrix
	var A00, a01, A02, a10, a11, a12, A20, a21, A22 cmat.FloatMatrix

	csource := cmat.NewFloatConstSource(1.0)
	A := cmat.NewMatrix(6, 6)
	As.SubMatrix(A, 1, 1, 4, 4)
	As.SetFrom(csource)

	Partition2x2(&ATL, &ATR, &ABL, &ABR, &As, 0, 0, PTOPLEFT)
	t.Logf("ATL:\n%v\n", &ATL)

	t.Logf("n(ATL)=%d, n(As)=%d\n", n(&ATL), n(&As))
	k := 0
	for n(&ATL) < n(&As) && k < n(&As) {
		Repartition2x2to3x3(&ATL,
			&A00, &a01, &A02,
			&a10, &a11, &a12,
			&A20, &a21, &A22, &As, 1, PBOTTOMRIGHT)
		t.Logf("n(A00)=%d, n(a01)=%d, n(A02)=%d\n", n(&A00), n(&a01), n(&A02))
		t.Logf("n(a10)=%d, n(a11)=%d, n(a12)=%d\n", n(&a10), n(&a11), n(&a12))
		t.Logf("n(A20)=%d, n(a21)=%d, n(A22)=%d\n", n(&A20), n(&a21), n(&A22))
		//t.Logf("n(a12)=%d [%d], n(a11)=%d\n", n(&a12), a12.Len(), a11.Len())
		a11.Set(0, 0, a11.Get(0, 0)+1.0)
		addConst(&a21, -2.0)

		Continue3x3to2x2(&ATL, &ATR, &ABL, &ABR, &A00, &a11, &A22, &As, PBOTTOMRIGHT)
		t.Logf("n(ATL)=%d, n(As)=%d\n", n(&ATL), n(&As))
		k += 1
	}
	t.Logf("A:\n%v\n", A)
}

// test: min ||X|| s.t A.T*X = B
func TestSolveQR(t *testing.T) {
	M := 799
	N := 711
	K := 241
	nb := 32
	conf := gomas.NewConf()
	conf.LB = nb

	tau := cmat.NewMatrix(N, 1)
	A := cmat.NewMatrix(M, N)
	src := cmat.NewFloatNormSource()
	A.SetFrom(src)
	A0 := cmat.NewCopy(A)
	B0 := cmat.NewMatrix(M, K)
	B0.SetFrom(src)
	B := cmat.NewCopy(B0)

	W := lapackd.Workspace(lapackd.QRFactorWork(A, conf))
	lapackd.QRFactor(A, tau, W, conf)

	lapackd.QRSolve(B, A, tau, W, gomas.TRANS, conf)

	var Bmin cmat.FloatMatrix
	Bmin.SubMatrix(B0, 0, 0, N, K)
	blasd.Mult(&Bmin, A0, B, 1.0, -1.0, gomas.TRANSA, conf)

	nrm := lapackd.NormP(&Bmin, lapackd.NORM_ONE)
	t.Logf("M=%d, N=%d ||B - A.T*X||_1: %e\n", M, N, nrm)
}

// unblocked LU decomposition w/o pivots, FLAME LU nopivots variant 5
func unblockedLUnoPiv(A *cmat.FloatMatrix, conf *gomas.Config) *gomas.Error {
	var ATL, ATR, ABL, ABR cmat.FloatMatrix
	var A00, a01, A02, a10, a11, a12, A20, a21, A22 cmat.FloatMatrix
	var err *gomas.Error = nil

	util.Partition2x2(
		&ATL, &ATR,
		&ABL, &ABR, A, 0, 0, util.PTOPLEFT)

	for m(&ATL) < m(A) {
		util.Repartition2x2to3x3(&ATL,
			&A00, &a01, &A02,
			&a10, &a11, &a12,
			&A20, &a21, &A22, A, 1, util.PBOTTOMRIGHT)

		// a21 = a21/a11
		blasd.InvScale(&a21, a11.Get(0, 0))
		// A22 = A22 - a21*a12
		blasd.MVUpdate(&A22, &a21, &a12, -1.0)

		util.Continue3x3to2x2(
			&ATL, &ATR,
			&ABL, &ABR, &A00, &a11, &A22, A, util.PBOTTOMRIGHT)
	}
	return err
}

func minvscale(A *cmat.FloatMatrix, alpha float64, M, N int) {
	var a C.mdata_t

	a.md = (*C.double)(unsafe.Pointer(&A.Data()[0]))
	a.step = C.int(A.Stride())
	C.__d_blk_invscale(
		(*C.mdata_t)(unsafe.Pointer(&a)), C.double(alpha), C.int(M), C.int(N))
	return
}

func Transpose(A, B *cmat.FloatMatrix, confs ...*gomas.Config) *gomas.Error {
	ar, ac := A.Size()
	br, bc := B.Size()
	if ar != bc || ac != br {
		return gomas.NewError(gomas.ESIZE, "Transpose")
	}
	mtranspose(A, B, br, bc)
	return nil
}

/*
 * Build block reflector for QL factorized matrix.
 */
func QLReflector(T, A, tau *cmat.FloatMatrix, confs ...*gomas.Config) *gomas.Error {
	var tauh cmat.FloatMatrix

	if n(T) < n(A) || m(T) < n(A) {
		return gomas.NewError(gomas.ESIZE, "QLReflector")
	}

	tauh.SubMatrix(tau, 0, 0, imin(m(A), n(A)), 1)
	unblkQLBlockReflector(T, A, &tauh)
	return nil
}

// Compute rational approximation backward
func rationalBackward(z, delta *cmat.FloatMatrix, start, end int) (float64, float64) {
	var val, dval float64
	val, dval = 0.0, 0.0
	for i := end - 1; i >= start; i-- {
		dj := delta.GetAtUnsafe(i)
		zj := z.GetAtUnsafe(i)
		tval := zj / dj
		val = val + zj*tval
		dval = dval + tval*tval
	}
	return val, dval
}

func mNormInf(A *cmat.FloatMatrix) float64 {
	var amax float64 = 0.0
	var row cmat.FloatMatrix
	arows, _ := A.Size()
	for k := 0; k < arows; k++ {
		row.Row(A, k)
		rmax := blasd.ASum(&row)
		if rmax > amax {
			amax = rmax
		}
	}
	return amax
}

func mNorm1(A *cmat.FloatMatrix) float64 {
	var amax float64 = 0.0
	var col cmat.FloatMatrix
	_, acols := A.Size()
	for k := 0; k < acols; k++ {
		col.Column(A, k)
		cmax := blasd.ASum(&col)
		if cmax > amax {
			amax = cmax
		}
	}
	return amax
}

// Computes eigenvectors corresponding the updated eigenvalues and rank-one update vector.
// The matrix Qd holds precomputed deltas as returned by TRDSecularSolveAll(). If Qd is nil or
// Qd same as the matrix Q then computation is in-place and Q is assumed to hold precomputed
// deltas. On exit, Q holds the column eigenvectors.
func TRDSecularEigen(Q, v, Qd *cmat.FloatMatrix, confs ...*gomas.Config) *gomas.Error {
	if m(Q) != n(Q) || (Qd != nil && (m(Qd) != n(Qd) || m(Qd) != m(Q))) {
		return gomas.NewError(gomas.ESIZE, "TRDSecularEigen")
	}
	if m(Q) != v.Len() {
		return gomas.NewError(gomas.ESIZE, "TRDSecularEigen")
	}
	if Qd == nil || Qd == Q {
		trdsecEigenBuildInplace(Q, v)
	} else {
		trdsecEigenBuild(Q, v, Qd)
	}
	return nil
}

func TestPartition1H(t *testing.T) {
	var AL, AR, A0, a1, A2 cmat.FloatMatrix
	A := cmat.NewMatrix(1, 6)
	Partition1x2(&AL, &AR, A, 0, PLEFT)
	t.Logf("n(AL)=%d, n(AR)=%d\n", n(&AL), n(&AR))
	for n(&AL) < n(A) {
		addConst(&AR, 1.0)
		t.Logf("n(AR)=%d; %v\n", n(&AR), &AR)
		Repartition1x2to1x3(&AL, &A0, &a1, &A2, A, 1, PRIGHT)
		t.Logf("n(A0)=%d, n(A2)=%d, a1=%.1f\n", n(&A0), n(&A2), a1.Get(0, 0))
		Continue1x3to1x2(&AL, &AR, &A0, &a1, A, PRIGHT)
	}
	t.Logf("A:%v\n", A)
}