Exemple #1
0
// Internal CPL solver for CP and CLP problems. Everything is wrapped to proper interfaces
func cpl_solver(F ConvexVarProg, c MatrixVariable, G MatrixVarG, h *matrix.FloatMatrix,
	A MatrixVarA, b MatrixVariable, dims *sets.DimensionSet, kktsolver KKTCpSolverVar,
	solopts *SolverOptions, x0 MatrixVariable, mnl int) (sol *Solution, err error) {

	const (
		STEP              = 0.99
		BETA              = 0.5
		ALPHA             = 0.01
		EXPON             = 3
		MAX_RELAXED_ITERS = 8
	)

	var refinement int

	sol = &Solution{Unknown,
		nil,
		0.0, 0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0, 0.0, 0}

	feasTolerance := FEASTOL
	absTolerance := ABSTOL
	relTolerance := RELTOL
	maxIter := MAXITERS
	if solopts.FeasTol > 0.0 {
		feasTolerance = solopts.FeasTol
	}
	if solopts.AbsTol > 0.0 {
		absTolerance = solopts.AbsTol
	}
	if solopts.RelTol > 0.0 {
		relTolerance = solopts.RelTol
	}
	if solopts.Refinement > 0 {
		refinement = solopts.Refinement
	} else {
		refinement = 1
	}
	if solopts.MaxIter > 0 {
		maxIter = solopts.MaxIter
	}

	if x0 == nil {
		mnl, x0, err = F.F0()
		if err != nil {
			return
		}
	}

	if c == nil {
		err = errors.New("Must define objective.")
		return
	}

	if h == nil {
		h = matrix.FloatZeros(0, 1)
	}
	if dims == nil {
		err = errors.New("Problem dimensions not defined.")
		return
	}
	if err = checkConeLpDimensions(dims); err != nil {
		return
	}

	cdim := dims.Sum("l", "q") + dims.SumSquared("s")
	cdim_diag := dims.Sum("l", "q", "s")

	if h.Rows() != cdim {
		err = errors.New(fmt.Sprintf("'h' must be float matrix of size (%d,1)", cdim))
		return
	}

	if G == nil {
		err = errors.New("'G' must be non-nil MatrixG interface.")
		return
	}
	fG := func(x, y MatrixVariable, alpha, beta float64, trans la.Option) error {
		return G.Gf(x, y, alpha, beta, trans)
	}

	// Check A and set defaults if it is nil
	if A == nil {
		err = errors.New("'A' must be non-nil MatrixA interface.")
		return
	}
	fA := func(x, y MatrixVariable, alpha, beta float64, trans la.Option) error {
		return A.Af(x, y, alpha, beta, trans)
	}

	if b == nil {
		err = errors.New("'b' must be non-nil MatrixVariable interface.")
		return
	}

	if kktsolver == nil {
		err = errors.New("nil kktsolver not allowed.")
		return
	}

	x := x0.Copy()
	y := b.Copy()
	y.Scal(0.0)
	z := matrix.FloatZeros(mnl+cdim, 1)
	s := matrix.FloatZeros(mnl+cdim, 1)
	ind := mnl + dims.At("l")[0]
	z.SetIndexes(1.0, matrix.MakeIndexSet(0, ind, 1)...)
	s.SetIndexes(1.0, matrix.MakeIndexSet(0, ind, 1)...)
	for _, m := range dims.At("q") {
		z.SetIndexes(1.0, ind)
		s.SetIndexes(1.0, ind)
		ind += m
	}
	for _, m := range dims.At("s") {
		iset := matrix.MakeIndexSet(ind, ind+m*m, m+1)
		z.SetIndexes(1.0, iset...)
		s.SetIndexes(1.0, iset...)
		ind += m * m
	}

	rx := x0.Copy()
	ry := b.Copy()
	dx := x.Copy()
	dy := y.Copy()
	rznl := matrix.FloatZeros(mnl, 1)
	rzl := matrix.FloatZeros(cdim, 1)
	dz := matrix.FloatZeros(mnl+cdim, 1)
	ds := matrix.FloatZeros(mnl+cdim, 1)
	lmbda := matrix.FloatZeros(mnl+cdim_diag, 1)
	lmbdasq := matrix.FloatZeros(mnl+cdim_diag, 1)
	sigs := matrix.FloatZeros(dims.Sum("s"), 1)
	sigz := matrix.FloatZeros(dims.Sum("s"), 1)

	dz2 := matrix.FloatZeros(mnl+cdim, 1)
	ds2 := matrix.FloatZeros(mnl+cdim, 1)

	newx := x.Copy()
	newy := y.Copy()
	newrx := x0.Copy()

	newz := matrix.FloatZeros(mnl+cdim, 1)
	news := matrix.FloatZeros(mnl+cdim, 1)
	newrznl := matrix.FloatZeros(mnl, 1)

	rx0 := rx.Copy()
	ry0 := ry.Copy()
	rznl0 := matrix.FloatZeros(mnl, 1)
	rzl0 := matrix.FloatZeros(cdim, 1)

	x0, dx0 := x.Copy(), dx.Copy()
	y0, dy0 := y.Copy(), dy.Copy()

	z0 := matrix.FloatZeros(mnl+cdim, 1)
	dz0 := matrix.FloatZeros(mnl+cdim, 1)
	dz20 := matrix.FloatZeros(mnl+cdim, 1)

	s0 := matrix.FloatZeros(mnl+cdim, 1)
	ds0 := matrix.FloatZeros(mnl+cdim, 1)
	ds20 := matrix.FloatZeros(mnl+cdim, 1)

	checkpnt.AddMatrixVar("z", z)
	checkpnt.AddMatrixVar("s", s)
	checkpnt.AddMatrixVar("dz", dz)
	checkpnt.AddMatrixVar("ds", ds)
	checkpnt.AddMatrixVar("rznl", rznl)
	checkpnt.AddMatrixVar("rzl", rzl)
	checkpnt.AddMatrixVar("lmbda", lmbda)
	checkpnt.AddMatrixVar("lmbdasq", lmbdasq)
	checkpnt.AddMatrixVar("z0", z0)
	checkpnt.AddMatrixVar("dz0", dz0)
	checkpnt.AddVerifiable("c", c)
	checkpnt.AddVerifiable("x", x)
	checkpnt.AddVerifiable("rx", rx)
	checkpnt.AddVerifiable("dx", dx)
	checkpnt.AddVerifiable("newrx", newrx)
	checkpnt.AddVerifiable("newx", newx)
	checkpnt.AddVerifiable("x0", x0)
	checkpnt.AddVerifiable("dx0", dx0)
	checkpnt.AddVerifiable("rx0", rx0)
	checkpnt.AddVerifiable("y", y)
	checkpnt.AddVerifiable("dy", dy)

	W0 := sets.NewFloatSet("d", "di", "dnl", "dnli", "v", "r", "rti", "beta")
	W0.Set("dnl", matrix.FloatZeros(mnl, 1))
	W0.Set("dnli", matrix.FloatZeros(mnl, 1))
	W0.Set("d", matrix.FloatZeros(dims.At("l")[0], 1))
	W0.Set("di", matrix.FloatZeros(dims.At("l")[0], 1))
	W0.Set("beta", matrix.FloatZeros(len(dims.At("q")), 1))
	for _, n := range dims.At("q") {
		W0.Append("v", matrix.FloatZeros(n, 1))
	}
	for _, n := range dims.At("s") {
		W0.Append("r", matrix.FloatZeros(n, n))
		W0.Append("rti", matrix.FloatZeros(n, n))
	}
	lmbda0 := matrix.FloatZeros(mnl+dims.Sum("l", "q", "s"), 1)
	lmbdasq0 := matrix.FloatZeros(mnl+dims.Sum("l", "q", "s"), 1)

	var f MatrixVariable = nil
	var Df MatrixVarDf = nil
	var H MatrixVarH = nil

	var ws3, wz3, wz2l, wz2nl *matrix.FloatMatrix
	var ws, wz, wz2, ws2 *matrix.FloatMatrix
	var wx, wx2, wy, wy2 MatrixVariable
	var gap, gap0, theta1, theta2, theta3, ts, tz, phi, phi0, mu, sigma, eta float64
	var resx, resy, reszl, resznl, pcost, dcost, dres, pres, relgap float64
	var resx0, resznl0, dres0, pres0 float64
	var dsdz, dsdz0, step, step0, dphi, dphi0, sigma0, eta0 float64
	var newresx, newresznl, newgap, newphi float64
	var W *sets.FloatMatrixSet
	var f3 KKTFuncVar

	checkpnt.AddFloatVar("gap", &gap)
	checkpnt.AddFloatVar("pcost", &pcost)
	checkpnt.AddFloatVar("dcost", &dcost)
	checkpnt.AddFloatVar("pres", &pres)
	checkpnt.AddFloatVar("dres", &dres)
	checkpnt.AddFloatVar("relgap", &relgap)
	checkpnt.AddFloatVar("step", &step)
	checkpnt.AddFloatVar("dsdz", &dsdz)
	checkpnt.AddFloatVar("resx", &resx)
	checkpnt.AddFloatVar("resy", &resy)
	checkpnt.AddFloatVar("reszl", &reszl)
	checkpnt.AddFloatVar("resznl", &resznl)

	// Declare fDf and fH here, they bind to Df and H as they are already declared.
	// ??really??

	var fDf func(u, v MatrixVariable, alpha, beta float64, trans la.Option) error = nil
	var fH func(u, v MatrixVariable, alpha, beta float64) error = nil

	relaxed_iters := 0
	for iters := 0; iters <= maxIter+1; iters++ {
		checkpnt.MajorNext()
		checkpnt.Check("loopstart", 10)

		checkpnt.MinorPush(10)
		if refinement != 0 || solopts.Debug {
			f, Df, H, err = F.F2(x, matrix.FloatVector(z.FloatArray()[:mnl]))
			fDf = func(u, v MatrixVariable, alpha, beta float64, trans la.Option) error {
				return Df.Df(u, v, alpha, beta, trans)
			}
			fH = func(u, v MatrixVariable, alpha, beta float64) error {
				return H.Hf(u, v, alpha, beta)
			}
		} else {
			f, Df, err = F.F1(x)
			fDf = func(u, v MatrixVariable, alpha, beta float64, trans la.Option) error {
				return Df.Df(u, v, alpha, beta, trans)
			}
		}
		checkpnt.MinorPop()

		gap = sdot(s, z, dims, mnl)

		// these are helpers, copies of parts of z,s
		z_mnl := matrix.FloatVector(z.FloatArray()[:mnl])
		z_mnl2 := matrix.FloatVector(z.FloatArray()[mnl:])
		s_mnl := matrix.FloatVector(s.FloatArray()[:mnl])
		s_mnl2 := matrix.FloatVector(s.FloatArray()[mnl:])

		// rx = c + A'*y + Df'*z[:mnl] + G'*z[mnl:]
		// -- y, rx MatrixArg
		mCopy(c, rx)
		fA(y, rx, 1.0, 1.0, la.OptTrans)
		fDf(&matrixVar{z_mnl}, rx, 1.0, 1.0, la.OptTrans)
		fG(&matrixVar{z_mnl2}, rx, 1.0, 1.0, la.OptTrans)
		resx = math.Sqrt(rx.Dot(rx))

		// rznl = s[:mnl] + f
		blas.Copy(s_mnl, rznl)
		blas.AxpyFloat(f.Matrix(), rznl, 1.0)
		resznl = blas.Nrm2Float(rznl)

		// rzl = s[mnl:] + G*x - h
		blas.Copy(s_mnl2, rzl)
		blas.AxpyFloat(h, rzl, -1.0)
		fG(x, &matrixVar{rzl}, 1.0, 1.0, la.OptNoTrans)
		reszl = snrm2(rzl, dims, 0)

		// Statistics for stopping criteria
		// pcost = c'*x
		// dcost = c'*x + y'*(A*x-b) + znl'*f(x) + zl'*(G*x-h)
		//       = c'*x + y'*(A*x-b) + znl'*(f(x)+snl) + zl'*(G*x-h+sl)
		//         - z'*s
		//       = c'*x + y'*ry + znl'*rznl + zl'*rzl - gap
		//pcost = blas.DotFloat(c, x)
		pcost = c.Dot(x)
		dcost = pcost + blas.DotFloat(y.Matrix(), ry.Matrix()) + blas.DotFloat(z_mnl, rznl)
		dcost += sdot(z_mnl2, rzl, dims, 0) - gap

		if pcost < 0.0 {
			relgap = gap / -pcost
		} else if dcost > 0.0 {
			relgap = gap / dcost
		} else {
			relgap = math.NaN()
		}
		pres = math.Sqrt(resy*resy + resznl*resznl + reszl*reszl)
		dres = resx
		if iters == 0 {
			resx0 = math.Max(1.0, resx)
			resznl0 = math.Max(1.0, resznl)
			pres0 = math.Max(1.0, pres)
			dres0 = math.Max(1.0, dres)
			gap0 = gap
			theta1 = 1.0 / gap0
			theta2 = 1.0 / resx0
			theta3 = 1.0 / resznl0
		}
		phi = theta1*gap + theta2*resx + theta3*resznl
		pres = pres / pres0
		dres = dres / dres0

		if solopts.ShowProgress {
			if iters == 0 {
				// some headers
				fmt.Printf("% 10s% 12s% 10s% 8s% 7s\n",
					"pcost", "dcost", "gap", "pres", "dres")
			}
			fmt.Printf("%2d: % 8.4e % 8.4e % 4.0e% 7.0e% 7.0e\n",
				iters, pcost, dcost, gap, pres, dres)
		}

		checkpnt.Check("checkgap", 50)
		// Stopping criteria
		if (pres <= feasTolerance && dres <= feasTolerance &&
			(gap <= absTolerance || (!math.IsNaN(relgap) && relgap <= relTolerance))) ||
			iters == maxIter {

			if iters == maxIter {
				s := "Terminated (maximum number of iterations reached)"
				if solopts.ShowProgress {
					fmt.Printf(s + "\n")
				}
				err = errors.New(s)
				sol.Status = Unknown
			} else {
				err = nil
				sol.Status = Optimal
			}
			sol.Result = sets.NewFloatSet("x", "y", "znl", "zl", "snl", "sl")
			sol.Result.Set("x", x.Matrix())
			sol.Result.Set("y", y.Matrix())
			sol.Result.Set("znl", matrix.FloatVector(z.FloatArray()[:mnl]))
			sol.Result.Set("zl", matrix.FloatVector(z.FloatArray()[mnl:]))
			sol.Result.Set("sl", matrix.FloatVector(s.FloatArray()[mnl:]))
			sol.Result.Set("snl", matrix.FloatVector(s.FloatArray()[:mnl]))
			sol.Gap = gap
			sol.RelativeGap = relgap
			sol.PrimalObjective = pcost
			sol.DualObjective = dcost
			sol.PrimalInfeasibility = pres
			sol.DualInfeasibility = dres
			sol.PrimalSlack = -ts
			sol.DualSlack = -tz
			return
		}

		// Compute initial scaling W:
		//
		//     W * z = W^{-T} * s = lambda.
		//
		// lmbdasq = lambda o lambda
		if iters == 0 {
			W, _ = computeScaling(s, z, lmbda, dims, mnl)
			checkpnt.AddScaleVar(W)
		}
		ssqr(lmbdasq, lmbda, dims, mnl)
		checkpnt.Check("lmbdasq", 90)

		// f3(x, y, z) solves
		//
		//     [ H   A'  GG'*W^{-1} ] [ ux ]   [ bx ]
		//     [ A   0   0          ] [ uy ] = [ by ].
		//     [ GG  0  -W'         ] [ uz ]   [ bz ]
		//
		// On entry, x, y, z contain bx, by, bz.
		// On exit, they contain ux, uy, uz.
		checkpnt.MinorPush(95)
		f3, err = kktsolver(W, x, z_mnl)
		checkpnt.MinorPop()
		checkpnt.Check("f3", 100)
		if err != nil {
			// ?? z_mnl is really copy of z[:mnl] ... should we copy here back to z??
			singular_kkt_matrix := false
			if iters == 0 {
				err = errors.New("Rank(A) < p or Rank([H(x); A; Df(x); G] < n")
				return
			} else if relaxed_iters > 0 && relaxed_iters < MAX_RELAXED_ITERS {
				// The arithmetic error may be caused by a relaxed line
				// search in the previous iteration.  Therefore we restore
				// the last saved state and require a standard line search.
				phi, gap = phi0, gap0
				mu = gap / float64(mnl+dims.Sum("l", "s")+len(dims.At("q")))
				blas.Copy(W0.At("dnl")[0], W.At("dnl")[0])
				blas.Copy(W0.At("dnli")[0], W.At("dnli")[0])
				blas.Copy(W0.At("d")[0], W.At("d")[0])
				blas.Copy(W0.At("di")[0], W.At("di")[0])
				blas.Copy(W0.At("beta")[0], W.At("beta")[0])
				for k, _ := range dims.At("q") {
					blas.Copy(W0.At("v")[k], W.At("v")[k])
				}
				for k, _ := range dims.At("s") {
					blas.Copy(W0.At("r")[k], W.At("r")[k])
					blas.Copy(W0.At("rti")[k], W.At("rti")[k])
				}
				//blas.Copy(x0, x)
				//x0.CopyTo(x)
				mCopy(x0, x)
				//blas.Copy(y0, y)
				mCopy(y0, y)
				blas.Copy(s0, s)
				blas.Copy(z0, z)
				blas.Copy(lmbda0, lmbda)
				blas.Copy(lmbdasq0, lmbdasq) // ???
				//blas.Copy(rx0, rx)
				//rx0.CopyTo(rx)
				mCopy(rx0, rx)
				//blas.Copy(ry0, ry)
				mCopy(ry0, ry)
				//resx = math.Sqrt(blas.DotFloat(rx, rx))
				resx = math.Sqrt(rx.Dot(rx))
				blas.Copy(rznl0, rznl)
				blas.Copy(rzl0, rzl)
				resznl = blas.Nrm2Float(rznl)

				relaxed_iters = -1

				// How about z_mnl here???
				checkpnt.MinorPush(120)
				f3, err = kktsolver(W, x, z_mnl)
				checkpnt.MinorPop()
				if err != nil {
					singular_kkt_matrix = true
				}
			} else {
				singular_kkt_matrix = true
			}

			if singular_kkt_matrix {
				msg := "Terminated (singular KKT matrix)."
				if solopts.ShowProgress {
					fmt.Printf(msg + "\n")
				}
				zl := matrix.FloatVector(z.FloatArray()[mnl:])
				sl := matrix.FloatVector(s.FloatArray()[mnl:])
				ind := dims.Sum("l", "q")
				for _, m := range dims.At("s") {
					symm(sl, m, ind)
					symm(zl, m, ind)
					ind += m * m
				}
				ts, _ = maxStep(s, dims, mnl, nil)
				tz, _ = maxStep(z, dims, mnl, nil)

				err = errors.New(msg)
				sol.Status = Unknown
				sol.Result = sets.NewFloatSet("x", "y", "znl", "zl", "snl", "sl")
				sol.Result.Set("x", x.Matrix())
				sol.Result.Set("y", y.Matrix())
				sol.Result.Set("znl", matrix.FloatVector(z.FloatArray()[:mnl]))
				sol.Result.Set("zl", zl)
				sol.Result.Set("sl", sl)
				sol.Result.Set("snl", matrix.FloatVector(s.FloatArray()[:mnl]))
				sol.Gap = gap
				sol.RelativeGap = relgap
				sol.PrimalObjective = pcost
				sol.DualObjective = dcost
				sol.PrimalInfeasibility = pres
				sol.DualInfeasibility = dres
				sol.PrimalSlack = -ts
				sol.DualSlack = -tz
				return
			}
		}

		// f4_no_ir(x, y, z, s) solves
		//
		//     [ 0     ]   [ H   A'  GG' ] [ ux        ]   [ bx ]
		//     [ 0     ] + [ A   0   0   ] [ uy        ] = [ by ]
		//     [ W'*us ]   [ GG  0   0   ] [ W^{-1}*uz ]   [ bz ]
		//
		//     lmbda o (uz + us) = bs.
		//
		// On entry, x, y, z, x, contain bx, by, bz, bs.
		// On exit, they contain ux, uy, uz, us.

		if iters == 0 {
			ws3 = matrix.FloatZeros(mnl+cdim, 1)
			wz3 = matrix.FloatZeros(mnl+cdim, 1)
			checkpnt.AddMatrixVar("ws3", ws3)
			checkpnt.AddMatrixVar("wz3", wz3)
		}

		f4_no_ir := func(x, y MatrixVariable, z, s *matrix.FloatMatrix) (err error) {
			// Solve
			//
			//     [ H  A'  GG'  ] [ ux        ]   [ bx                    ]
			//     [ A  0   0    ] [ uy        ] = [ by                    ]
			//     [ GG 0  -W'*W ] [ W^{-1}*uz ]   [ bz - W'*(lmbda o\ bs) ]
			//
			//     us = lmbda o\ bs - uz.

			err = nil
			// s := lmbda o\ s
			//    = lmbda o\ bs
			sinv(s, lmbda, dims, mnl)

			// z := z - W'*s
			//    = bz - W' * (lambda o\ bs)
			blas.Copy(s, ws3)

			scale(ws3, W, true, false)
			blas.AxpyFloat(ws3, z, -1.0)

			// Solve for ux, uy, uz
			err = f3(x, y, z)

			// s := s - z
			//    = lambda o\ bs - z.
			blas.AxpyFloat(z, s, -1.0)
			return
		}

		if iters == 0 {
			wz2nl = matrix.FloatZeros(mnl, 1)
			wz2l = matrix.FloatZeros(cdim, 1)
			checkpnt.AddMatrixVar("wz2nl", wz2nl)
			checkpnt.AddMatrixVar("wz2l", wz2l)
		}

		res := func(ux, uy MatrixVariable, uz, us *matrix.FloatMatrix, vx, vy MatrixVariable, vz, vs *matrix.FloatMatrix) (err error) {

			// Evaluates residuals in Newton equations:
			//
			//     [ vx ]     [ 0     ]   [ H  A' GG' ] [ ux        ]
			//     [ vy ] -=  [ 0     ] + [ A  0  0   ] [ uy        ]
			//     [ vz ]     [ W'*us ]   [ GG 0  0   ] [ W^{-1}*uz ]
			//
			//     vs -= lmbda o (uz + us).
			err = nil
			minor := checkpnt.MinorTop()
			// vx := vx - H*ux - A'*uy - GG'*W^{-1}*uz
			fH(ux, vx, -1.0, 1.0)
			fA(uy, vx, -1.0, 1.0, la.OptTrans)
			blas.Copy(uz, wz3)
			scale(wz3, W, false, true)
			wz3_nl := matrix.FloatVector(wz3.FloatArray()[:mnl])
			wz3_l := matrix.FloatVector(wz3.FloatArray()[mnl:])
			fDf(&matrixVar{wz3_nl}, vx, -1.0, 1.0, la.OptTrans)
			fG(&matrixVar{wz3_l}, vx, -1.0, 1.0, la.OptTrans)

			checkpnt.Check("10res", minor+10)

			// vy := vy - A*ux
			fA(ux, vy, -1.0, 1.0, la.OptNoTrans)

			// vz := vz - W'*us - GG*ux
			err = fDf(ux, &matrixVar{wz2nl}, 1.0, 0.0, la.OptNoTrans)
			checkpnt.Check("15res", minor+10)
			blas.AxpyFloat(wz2nl, vz, -1.0)
			fG(ux, &matrixVar{wz2l}, 1.0, 0.0, la.OptNoTrans)
			checkpnt.Check("20res", minor+10)
			blas.AxpyFloat(wz2l, vz, -1.0, &la.IOpt{"offsety", mnl})
			blas.Copy(us, ws3)
			scale(ws3, W, true, false)
			blas.AxpyFloat(ws3, vz, -1.0)

			checkpnt.Check("30res", minor+10)

			// vs -= lmbda o (uz + us)
			blas.Copy(us, ws3)
			blas.AxpyFloat(uz, ws3, 1.0)
			sprod(ws3, lmbda, dims, mnl, &la.SOpt{"diag", "D"})
			blas.AxpyFloat(ws3, vs, -1.0)

			checkpnt.Check("90res", minor+10)
			return
		}

		// f4(x, y, z, s) solves the same system as f4_no_ir, but applies
		// iterative refinement.

		if iters == 0 {
			if refinement > 0 || solopts.Debug {
				wx = c.Copy()
				wy = b.Copy()
				wz = z.Copy()
				ws = s.Copy()
				checkpnt.AddVerifiable("wx", wx)
				checkpnt.AddMatrixVar("ws", ws)
				checkpnt.AddMatrixVar("wz", wz)
			}
			if refinement > 0 {
				wx2 = c.Copy()
				wy2 = b.Copy()
				wz2 = matrix.FloatZeros(mnl+cdim, 1)
				ws2 = matrix.FloatZeros(mnl+cdim, 1)
				checkpnt.AddVerifiable("wx2", wx2)
				checkpnt.AddMatrixVar("ws2", ws2)
				checkpnt.AddMatrixVar("wz2", wz2)
			}
		}

		f4 := func(x, y MatrixVariable, z, s *matrix.FloatMatrix) (err error) {
			if refinement > 0 || solopts.Debug {
				mCopy(x, wx)
				mCopy(y, wy)
				blas.Copy(z, wz)
				blas.Copy(s, ws)
			}
			minor := checkpnt.MinorTop()
			checkpnt.Check("0_f4", minor+100)
			checkpnt.MinorPush(minor + 100)

			err = f4_no_ir(x, y, z, s)

			checkpnt.MinorPop()
			checkpnt.Check("1_f4", minor+200)
			for i := 0; i < refinement; i++ {
				mCopy(wx, wx2)
				mCopy(wy, wy2)
				blas.Copy(wz, wz2)
				blas.Copy(ws, ws2)

				checkpnt.Check("2_f4", minor+(1+i)*200)
				checkpnt.MinorPush(minor + (1+i)*200)

				res(x, y, z, s, wx2, wy2, wz2, ws2)
				checkpnt.MinorPop()
				checkpnt.Check("3_f4", minor+(1+i)*200+100)

				err = f4_no_ir(wx2, wy2, wz2, ws2)
				checkpnt.MinorPop()
				checkpnt.Check("4_f4", minor+(1+i)*200+199)
				wx2.Axpy(x, 1.0)
				wy2.Axpy(y, 1.0)
				blas.AxpyFloat(wz2, z, 1.0)
				blas.AxpyFloat(ws2, s, 1.0)
			}
			if solopts.Debug {
				res(x, y, z, s, wx, wy, wz, ws)
				fmt.Printf("KKT residuals:\n")
			}
			return
		}

		sigma, eta = 0.0, 0.0

		for i := 0; i < 2; i++ {
			minor := (i + 2) * 1000
			checkpnt.MinorPush(minor)
			checkpnt.Check("loop01", minor)

			// Solve
			//
			//     [ 0     ]   [ H  A' GG' ] [ dx        ]
			//     [ 0     ] + [ A  0  0   ] [ dy        ] = -(1 - eta)*r
			//     [ W'*ds ]   [ GG 0  0   ] [ W^{-1}*dz ]
			//
			//     lmbda o (dz + ds) = -lmbda o lmbda + sigma*mu*e.
			//

			mu = gap / float64(mnl+dims.Sum("l", "s")+len(dims.At("q")))
			blas.ScalFloat(ds, 0.0)
			blas.AxpyFloat(lmbdasq, ds, -1.0, &la.IOpt{"n", mnl + dims.Sum("l", "q")})

			ind = mnl + dims.At("l")[0]
			iset := matrix.MakeIndexSet(0, ind, 1)
			ds.Add(sigma*mu, iset...)
			for _, m := range dims.At("q") {
				ds.Add(sigma*mu, ind)
				ind += m
			}
			ind2 := ind
			for _, m := range dims.At("s") {
				blas.AxpyFloat(lmbdasq, ds, -1.0, &la.IOpt{"n", m}, &la.IOpt{"offsetx", ind2},
					&la.IOpt{"offsety", ind}, &la.IOpt{"incy", m + 1})
				ds.Add(sigma*mu, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
				ind2 += m
			}

			dx.Scal(0.0)
			rx.Axpy(dx, -1.0+eta)
			dy.Scal(0.0)
			ry.Axpy(dy, -1.0+eta)
			dz.Scale(0.0)
			blas.AxpyFloat(rznl, dz, -1.0+eta)
			blas.AxpyFloat(rzl, dz, -1.0+eta, &la.IOpt{"offsety", mnl})
			//fmt.Printf("dx=\n%v\n", dx)
			//fmt.Printf("dz=\n%v\n", dz.ToString("%.7f"))
			//fmt.Printf("ds=\n%v\n", ds.ToString("%.7f"))

			checkpnt.Check("pref4", minor)
			checkpnt.MinorPush(minor)
			err = f4(dx, dy, dz, ds)
			if err != nil {
				if iters == 0 {
					s := fmt.Sprintf("Rank(A) < p or Rank([H(x); A; Df(x); G] < n (%s)", err)
					err = errors.New(s)
					return
				}
				msg := "Terminated (singular KKT matrix)."
				if solopts.ShowProgress {
					fmt.Printf(msg + "\n")
				}
				zl := matrix.FloatVector(z.FloatArray()[mnl:])
				sl := matrix.FloatVector(s.FloatArray()[mnl:])
				ind := dims.Sum("l", "q")
				for _, m := range dims.At("s") {
					symm(sl, m, ind)
					symm(zl, m, ind)
					ind += m * m
				}
				ts, _ = maxStep(s, dims, mnl, nil)
				tz, _ = maxStep(z, dims, mnl, nil)

				err = errors.New(msg)
				sol.Status = Unknown
				sol.Result = sets.NewFloatSet("x", "y", "znl", "zl", "snl", "sl")
				sol.Result.Set("x", x.Matrix())
				sol.Result.Set("y", y.Matrix())
				sol.Result.Set("znl", matrix.FloatVector(z.FloatArray()[:mnl]))
				sol.Result.Set("zl", zl)
				sol.Result.Set("sl", sl)
				sol.Result.Set("snl", matrix.FloatVector(s.FloatArray()[:mnl]))
				sol.Gap = gap
				sol.RelativeGap = relgap
				sol.PrimalObjective = pcost
				sol.DualObjective = dcost
				sol.PrimalInfeasibility = pres
				sol.DualInfeasibility = dres
				sol.PrimalSlack = -ts
				sol.DualSlack = -tz
				return
			}

			checkpnt.MinorPop()
			checkpnt.Check("postf4", minor+400)

			// Inner product ds'*dz and unscaled steps are needed in the
			// line search.
			dsdz = sdot(ds, dz, dims, mnl)
			blas.Copy(dz, dz2)
			scale(dz2, W, false, true)
			blas.Copy(ds, ds2)
			scale(ds2, W, true, false)

			checkpnt.Check("dsdz", minor+400)

			// Maximum steps to boundary.
			//
			// Also compute the eigenvalue decomposition of 's' blocks in
			// ds, dz.  The eigenvectors Qs, Qz are stored in ds, dz.
			// The eigenvalues are stored in sigs, sigz.

			scale2(lmbda, ds, dims, mnl, false)
			ts, _ = maxStep(ds, dims, mnl, sigs)
			scale2(lmbda, dz, dims, mnl, false)
			tz, _ = maxStep(dz, dims, mnl, sigz)
			t := maxvec([]float64{0.0, ts, tz})
			if t == 0 {
				step = 1.0
			} else {
				step = math.Min(1.0, STEP/t)
			}

			checkpnt.Check("maxstep", minor+400)

			var newDf MatrixVarDf = nil
			var newf MatrixVariable = nil

			// Backtrack until newx is in domain of f.
			backtrack := true
			for backtrack {
				mCopy(x, newx)
				dx.Axpy(newx, step)
				newf, newDf, err = F.F1(newx)
				if newf != nil {
					backtrack = false
				} else {
					step *= BETA
				}
			}

			// Merit function
			//
			//     phi = theta1 * gap + theta2 * norm(rx) +
			//         theta3 * norm(rznl)
			//
			// and its directional derivative dphi.

			phi = theta1*gap + theta2*resx + theta3*resznl
			if i == 0 {
				dphi = -phi
			} else {
				dphi = -theta1*(1-sigma)*gap - theta2*(1-eta)*resx - theta3*(1-eta)*resznl
			}

			var newfDf func(x, y MatrixVariable, a, b float64, trans la.Option) error

			// Line search
			backtrack = true
			for backtrack {
				mCopy(x, newx)
				dx.Axpy(newx, step)
				mCopy(y, newy)
				dy.Axpy(newy, step)
				blas.Copy(z, newz)
				blas.AxpyFloat(dz2, newz, step)
				blas.Copy(s, news)
				blas.AxpyFloat(ds2, news, step)

				newf, newDf, err = F.F1(newx)
				newfDf = func(u, v MatrixVariable, a, b float64, trans la.Option) error {
					return newDf.Df(u, v, a, b, trans)
				}

				// newrx = c + A'*newy + newDf'*newz[:mnl] + G'*newz[mnl:]
				newz_mnl := matrix.FloatVector(newz.FloatArray()[:mnl])
				newz_ml := matrix.FloatVector(newz.FloatArray()[mnl:])
				//blas.Copy(c, newrx)
				//c.CopyTo(newrx)
				mCopy(c, newrx)
				fA(newy, newrx, 1.0, 1.0, la.OptTrans)
				newfDf(&matrixVar{newz_mnl}, newrx, 1.0, 1.0, la.OptTrans)
				fG(&matrixVar{newz_ml}, newrx, 1.0, 1.0, la.OptTrans)
				newresx = math.Sqrt(newrx.Dot(newrx))

				// newrznl = news[:mnl] + newf
				news_mnl := matrix.FloatVector(news.FloatArray()[:mnl])
				//news_ml := matrix.FloatVector(news.FloatArray()[mnl:])
				blas.Copy(news_mnl, newrznl)
				blas.AxpyFloat(newf.Matrix(), newrznl, 1.0)
				newresznl = blas.Nrm2Float(newrznl)

				newgap = (1.0-(1.0-sigma)*step)*gap + step*step*dsdz
				newphi = theta1*newgap + theta2*newresx + theta3*newresznl

				if i == 0 {
					if newgap <= (1.0-ALPHA*step)*gap &&
						(relaxed_iters > 0 && relaxed_iters < MAX_RELAXED_ITERS ||
							newphi <= phi+ALPHA*step*dphi) {
						backtrack = false
						sigma = math.Min(newgap/gap, math.Pow((newgap/gap), EXPON))
						//fmt.Printf("break 1: sigma=%.7f\n", sigma)
						eta = 0.0
					} else {
						step *= BETA
					}
				} else {
					if relaxed_iters == -1 || (relaxed_iters == 0 && MAX_RELAXED_ITERS == 0) {
						// Do a standard line search.
						if newphi <= phi+ALPHA*step*dphi {
							relaxed_iters = 0
							backtrack = false
							//fmt.Printf("break 2 : newphi=%.7f\n", newphi)
						} else {
							step *= BETA
						}
					} else if relaxed_iters == 0 && relaxed_iters < MAX_RELAXED_ITERS {
						if newphi <= phi+ALPHA*step*dphi {
							// Relaxed l.s. gives sufficient decrease.
							relaxed_iters = 0
						} else {
							// Save state.
							phi0, dphi0, gap0 = phi, dphi, gap
							step0 = step

							blas.Copy(W.At("dnl")[0], W0.At("dnl")[0])
							blas.Copy(W.At("dnli")[0], W0.At("dnli")[0])
							blas.Copy(W.At("d")[0], W0.At("d")[0])
							blas.Copy(W.At("di")[0], W0.At("di")[0])
							blas.Copy(W.At("beta")[0], W0.At("beta")[0])
							for k, _ := range dims.At("q") {
								blas.Copy(W.At("v")[k], W0.At("v")[k])
							}
							for k, _ := range dims.At("s") {
								blas.Copy(W.At("r")[k], W0.At("r")[k])
								blas.Copy(W.At("rti")[k], W0.At("rti")[k])
							}
							mCopy(x, x0)
							mCopy(y, y0)
							mCopy(dx, dx0)
							mCopy(dy, dy0)
							blas.Copy(s, s0)
							blas.Copy(z, z0)
							blas.Copy(ds, ds0)
							blas.Copy(dz, dz0)
							blas.Copy(ds2, ds20)
							blas.Copy(dz2, dz20)
							blas.Copy(lmbda, lmbda0)
							blas.Copy(lmbdasq, lmbdasq0) // ???
							mCopy(rx, rx0)
							mCopy(ry, ry0)
							blas.Copy(rznl, rznl0)
							blas.Copy(rzl, rzl0)
							dsdz0 = dsdz
							sigma0, eta0 = sigma, eta
							relaxed_iters = 1
						}
						backtrack = false
						//fmt.Printf("break 3 : newphi=%.7f\n", newphi)

					} else if relaxed_iters >= 0 && relaxed_iters < MAX_RELAXED_ITERS &&
						MAX_RELAXED_ITERS > 0 {
						if newphi <= phi0+ALPHA*step0*dphi0 {
							// Relaxed l.s. gives sufficient decrease.
							relaxed_iters = 0
						} else {
							// Relaxed line search
							relaxed_iters += 1
						}
						backtrack = false
						//fmt.Printf("break 4 : newphi=%.7f\n", newphi)

					} else if relaxed_iters == MAX_RELAXED_ITERS && MAX_RELAXED_ITERS > 0 {
						if newphi <= phi0+ALPHA*step0*dphi0 {
							// Series of relaxed line searches ends
							// with sufficient decrease w.r.t. phi0.
							backtrack = false
							relaxed_iters = 0
							//fmt.Printf("break 5 : newphi=%.7f\n", newphi)
						} else if newphi >= phi0 {
							// Resume last saved line search
							phi, dphi, gap = phi0, dphi0, gap0
							step = step0
							blas.Copy(W0.At("dnl")[0], W.At("dnl")[0])
							blas.Copy(W0.At("dnli")[0], W.At("dnli")[0])
							blas.Copy(W0.At("d")[0], W.At("d")[0])
							blas.Copy(W0.At("di")[0], W.At("di")[0])
							blas.Copy(W0.At("beta")[0], W.At("beta")[0])
							for k, _ := range dims.At("q") {
								blas.Copy(W0.At("v")[k], W.At("v")[k])
							}
							for k, _ := range dims.At("s") {
								blas.Copy(W0.At("r")[k], W.At("r")[k])
								blas.Copy(W0.At("rti")[k], W.At("rti")[k])
							}
							mCopy(x, x0)
							mCopy(y, y0)
							mCopy(dx, dx0)
							mCopy(dy, dy0)
							blas.Copy(s, s0)
							blas.Copy(z, z0)
							blas.Copy(ds2, ds20)
							blas.Copy(dz2, dz20)
							blas.Copy(lmbda, lmbda0)
							blas.Copy(lmbdasq, lmbdasq0) // ???
							mCopy(rx, rx0)
							mCopy(ry, ry0)
							blas.Copy(rznl, rznl0)
							blas.Copy(rzl, rzl0)
							dsdz = dsdz0
							sigma, eta = sigma0, eta0
							relaxed_iters = -1

						} else if newphi <= phi+ALPHA*step*dphi {
							// Series of relaxed line searches ends
							// with sufficient decrease w.r.t. phi0.
							backtrack = false
							relaxed_iters = -1
							//fmt.Printf("break 6 : newphi=%.7f\n", newphi)
						}
					}
				}
			} // end of line search

			checkpnt.Check("eol", minor+900)

		} // end for [0,1]

		// Update x, y
		dx.Axpy(x, step)
		dy.Axpy(y, step)
		checkpnt.Check("updatexy", 5000)

		// Replace nonlinear, 'l' and 'q' blocks of ds and dz with the
		// updated variables in the current scaling.
		// Replace 's' blocks of ds and dz with the factors Ls, Lz in a
		// factorization Ls*Ls', Lz*Lz' of the updated variables in the
		// current scaling.

		// ds := e + step*ds for nonlinear, 'l' and 'q' blocks.
		// dz := e + step*dz for nonlinear, 'l' and 'q' blocks.
		blas.ScalFloat(ds, step, &la.IOpt{"n", mnl + dims.Sum("l", "q")})
		blas.ScalFloat(dz, step, &la.IOpt{"n", mnl + dims.Sum("l", "q")})
		ind := mnl + dims.At("l")[0]
		is := matrix.MakeIndexSet(0, ind, 1)
		ds.Add(1.0, is...)
		dz.Add(1.0, is...)
		for _, m := range dims.At("q") {
			ds.SetIndex(ind, 1.0+ds.GetIndex(ind))
			dz.SetIndex(ind, 1.0+dz.GetIndex(ind))
			ind += m
		}
		checkpnt.Check("updatedsdz", 5100)

		// ds := H(lambda)^{-1/2} * ds and dz := H(lambda)^{-1/2} * dz.
		//
		// This replaces the 'l' and 'q' components of ds and dz with the
		// updated variables in the current scaling.
		// The 's' components of ds and dz are replaced with
		//
		// diag(lmbda_k)^{1/2} * Qs * diag(lmbda_k)^{1/2}
		// diag(lmbda_k)^{1/2} * Qz * diag(lmbda_k)^{1/2}
		scale2(lmbda, ds, dims, mnl, true)
		scale2(lmbda, dz, dims, mnl, true)

		checkpnt.Check("scale2", 5200)

		// sigs := ( e + step*sigs ) ./ lambda for 's' blocks.
		// sigz := ( e + step*sigz ) ./ lambda for 's' blocks.
		blas.ScalFloat(sigs, step)
		blas.ScalFloat(sigz, step)
		sigs.Add(1.0)
		sigz.Add(1.0)
		sdimsum := dims.Sum("s")
		qdimsum := dims.Sum("l", "q")
		blas.TbsvFloat(lmbda, sigs, &la.IOpt{"n", sdimsum}, &la.IOpt{"k", 0},
			&la.IOpt{"lda", 1}, &la.IOpt{"offseta", mnl + qdimsum})
		blas.TbsvFloat(lmbda, sigz, &la.IOpt{"n", sdimsum}, &la.IOpt{"k", 0},
			&la.IOpt{"lda", 1}, &la.IOpt{"offseta", mnl + qdimsum})

		checkpnt.Check("sigs", 5300)

		ind2 := mnl + qdimsum
		ind3 := 0
		sdims := dims.At("s")

		for k := 0; k < len(sdims); k++ {
			m := sdims[k]
			for i := 0; i < m; i++ {
				a := math.Sqrt(sigs.GetIndex(ind3 + i))
				blas.ScalFloat(ds, a, &la.IOpt{"offset", ind2 + m*i}, &la.IOpt{"n", m})
				a = math.Sqrt(sigz.GetIndex(ind3 + i))
				blas.ScalFloat(dz, a, &la.IOpt{"offset", ind2 + m*i}, &la.IOpt{"n", m})
			}
			ind2 += m * m
			ind3 += m
		}

		checkpnt.Check("scaling", 5400)
		err = updateScaling(W, lmbda, ds, dz)
		checkpnt.Check("postscaling", 5500)

		// Unscale s, z, tau, kappa (unscaled variables are used only to
		// compute feasibility residuals).
		ind = mnl + dims.Sum("l", "q")
		ind2 = ind
		blas.Copy(lmbda, s, &la.IOpt{"n", ind})
		for _, m := range dims.At("s") {
			blas.ScalFloat(s, 0.0, &la.IOpt{"offset", ind2})
			blas.Copy(lmbda, s, &la.IOpt{"offsetx", ind}, &la.IOpt{"offsety", ind2},
				&la.IOpt{"n", m}, &la.IOpt{"incy", m + 1})
			ind += m
			ind2 += m * m
		}
		scale(s, W, true, false)
		checkpnt.Check("unscale_s", 5600)

		ind = mnl + dims.Sum("l", "q")
		ind2 = ind
		blas.Copy(lmbda, z, &la.IOpt{"n", ind})
		for _, m := range dims.At("s") {
			blas.ScalFloat(z, 0.0, &la.IOpt{"offset", ind2})
			blas.Copy(lmbda, z, &la.IOpt{"offsetx", ind}, &la.IOpt{"offsety", ind2},
				&la.IOpt{"n", m}, &la.IOpt{"incy", m + 1})
			ind += m
			ind2 += m * m
		}
		scale(z, W, false, true)
		checkpnt.Check("unscale_z", 5700)

		gap = blas.DotFloat(lmbda, lmbda)

	}
	return
}
Exemple #2
0
func coneqp_solver(P MatrixVarP, q MatrixVariable, G MatrixVarG, h *matrix.FloatMatrix,
	A MatrixVarA, b MatrixVariable, dims *sets.DimensionSet, kktsolver KKTConeSolverVar,
	solopts *SolverOptions, initvals *sets.FloatMatrixSet) (sol *Solution, err error) {

	err = nil
	EXPON := 3
	STEP := 0.99

	sol = &Solution{Unknown,
		nil,
		0.0, 0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0, 0.0, 0}

	//var kktsolver func(*sets.FloatMatrixSet)(KKTFunc, error) = nil
	var refinement int
	var correction bool = true

	feasTolerance := FEASTOL
	absTolerance := ABSTOL
	relTolerance := RELTOL
	maxIter := MAXITERS
	if solopts.FeasTol > 0.0 {
		feasTolerance = solopts.FeasTol
	}
	if solopts.AbsTol > 0.0 {
		absTolerance = solopts.AbsTol
	}
	if solopts.RelTol > 0.0 {
		relTolerance = solopts.RelTol
	}
	if solopts.MaxIter > 0 {
		maxIter = solopts.MaxIter
	}
	if q == nil {
		err = errors.New("'q' must be non-nil MatrixVariable with one column")
		return
	}

	if h == nil {
		h = matrix.FloatZeros(0, 1)
	}
	if h.Cols() != 1 {
		err = errors.New("'h' must be non-nil matrix with one column")
		return
	}
	if dims == nil {
		dims = sets.NewDimensionSet("l", "q", "s")
		dims.Set("l", []int{h.Rows()})
	}

	err = checkConeQpDimensions(dims)
	if err != nil {
		return
	}

	cdim := dims.Sum("l", "q") + dims.SumSquared("s")
	//cdim_pckd := dims.Sum("l", "q") + dims.SumPacked("s")
	cdim_diag := dims.Sum("l", "q", "s")

	if h.Rows() != cdim {
		err = errors.New(fmt.Sprintf("'h' must be float matrix of size (%d,1)", cdim))
		return
	}

	// Data for kth 'q' constraint are found in rows indq[k]:indq[k+1] of G.
	indq := make([]int, 0)
	indq = append(indq, dims.At("l")[0])
	for _, k := range dims.At("q") {
		indq = append(indq, indq[len(indq)-1]+k)
	}

	// Data for kth 's' constraint are found in rows inds[k]:inds[k+1] of G.
	inds := make([]int, 0)
	inds = append(inds, indq[len(indq)-1])
	for _, k := range dims.At("s") {
		inds = append(inds, inds[len(inds)-1]+k*k)
	}

	if P == nil {
		err = errors.New("'P' must be non-nil MatrixVarP interface.")
		return
	}
	fP := func(u, v MatrixVariable, alpha, beta float64) error {
		return P.Pf(u, v, alpha, beta)
	}

	if G == nil {
		err = errors.New("'G' must be non-nil MatrixG interface.")
		return
	}
	fG := func(x, y MatrixVariable, alpha, beta float64, trans la.Option) error {
		return G.Gf(x, y, alpha, beta, trans)
	}

	// Check A and set defaults if it is nil
	fA := func(x, y MatrixVariable, alpha, beta float64, trans la.Option) error {
		return A.Af(x, y, alpha, beta, trans)
	}

	// Check b and set defaults if it is nil
	if b == nil {
		err = errors.New("'b' must be non-nil MatrixVariable interface.")
		return
	}

	// kktsolver(W) returns a routine for solving 3x3 block KKT system
	//
	//     [ 0   A'  G'*W^{-1} ] [ ux ]   [ bx ]
	//     [ A   0   0         ] [ uy ] = [ by ].
	//     [ G   0   -W'       ] [ uz ]   [ bz ]

	if kktsolver == nil {
		err = errors.New("nil kktsolver not allowed.")
		return
	}

	ws3 := matrix.FloatZeros(cdim, 1)
	wz3 := matrix.FloatZeros(cdim, 1)
	checkpnt.AddMatrixVar("ws3", ws3)
	checkpnt.AddMatrixVar("wz3", wz3)

	//
	res := func(ux, uy MatrixVariable, uz, us *matrix.FloatMatrix, vx, vy MatrixVariable, vz, vs *matrix.FloatMatrix, W *sets.FloatMatrixSet, lmbda *matrix.FloatMatrix) (err error) {
		// Evaluates residual in Newton equations:
		//
		//      [ vx ]    [ vx ]   [ 0     ]   [ P  A'  G' ]   [ ux        ]
		//      [ vy ] := [ vy ] - [ 0     ] - [ A  0   0  ] * [ uy        ]
		//      [ vz ]    [ vz ]   [ W'*us ]   [ G  0   0  ]   [ W^{-1}*uz ]
		//
		//      vs := vs - lmbda o (uz + us).

		// vx := vx - P*ux - A'*uy - G'*W^{-1}*uz
		minor := checkpnt.MinorTop()
		checkpnt.Check("00res", minor)
		fP(ux, vx, -1.0, 1.0)
		fA(uy, vx, -1.0, 1.0, la.OptTrans)
		blas.Copy(uz, wz3)
		scale(wz3, W, true, false)
		fG(&matrixVar{wz3}, vx, -1.0, 1.0, la.OptTrans)
		// vy := vy - A*ux
		fA(ux, vy, -1.0, 1.0, la.OptNoTrans)
		checkpnt.Check("50res", minor)

		// vz := vz - G*ux - W'*us
		fG(ux, &matrixVar{vz}, -1.0, 1.0, la.OptNoTrans)
		blas.Copy(us, ws3)
		scale(ws3, W, true, false)
		blas.AxpyFloat(ws3, vz, -1.0)

		// vs := vs - lmbda o (uz + us)
		blas.Copy(us, ws3)
		blas.AxpyFloat(uz, ws3, 1.0)
		sprod(ws3, lmbda, dims, 0, la.OptDiag)
		blas.AxpyFloat(ws3, vs, -1.0)
		checkpnt.Check("90res", minor)
		return
	}

	resx0 := math.Max(1.0, math.Sqrt(q.Dot(q)))
	resy0 := math.Max(1.0, math.Sqrt(b.Dot(b)))
	resz0 := math.Max(1.0, snrm2(h, dims, 0))
	//fmt.Printf("resx0: %.17f, resy0: %.17f, resz0: %.17f\n", resx0, resy0, resz0)

	var x, y, dx, dy, rx, ry MatrixVariable
	var z, s, ds, dz, rz *matrix.FloatMatrix
	var lmbda, lmbdasq, sigs, sigz *matrix.FloatMatrix
	var W *sets.FloatMatrixSet
	var f, f3 KKTFuncVar
	var resx, resy, resz, step, sigma, mu, eta float64
	var gap, pcost, dcost, relgap, pres, dres, f0 float64

	if cdim == 0 {
		// Solve
		//
		//     [ P  A' ] [ x ]   [ -q ]
		//     [       ] [   ] = [    ].
		//     [ A  0  ] [ y ]   [  b ]
		//
		Wtmp := sets.NewFloatSet("d", "di", "beta", "v", "r", "rti")
		Wtmp.Set("d", matrix.FloatZeros(0, 1))
		Wtmp.Set("di", matrix.FloatZeros(0, 1))
		f3, err = kktsolver(Wtmp)
		if err != nil {
			s := fmt.Sprintf("kkt error: %s", err)
			err = errors.New("2: Rank(A) < p or Rank(([P; A; G;]) < n : " + s)
			return
		}
		x = q.Copy()
		x.Scal(0.0)
		y = b.Copy()
		f3(x, y, matrix.FloatZeros(0, 1))

		// dres = || P*x + q + A'*y || / resx0
		rx = q.Copy()
		fP(x, rx, 1.0, 1.0)
		pcost = 0.5 * (x.Dot(rx) + x.Dot(q))
		fA(y, rx, 1.0, 1.0, la.OptTrans)
		dres = math.Sqrt(rx.Dot(rx) / resx0)

		ry = b.Copy()
		fA(x, ry, 1.0, -1.0, la.OptNoTrans)
		pres = math.Sqrt(ry.Dot(ry) / resy0)

		relgap = 0.0
		if pcost == 0.0 {
			relgap = math.NaN()
		}

		sol.Result = sets.NewFloatSet("x", "y", "s", "z")
		sol.Result.Set("x", x.Matrix())
		sol.Result.Set("y", y.Matrix())
		sol.Result.Set("s", matrix.FloatZeros(0, 1))
		sol.Result.Set("z", matrix.FloatZeros(0, 1))
		sol.Status = Optimal
		sol.Gap = 0.0
		sol.RelativeGap = relgap
		sol.PrimalObjective = pcost
		sol.DualObjective = pcost
		sol.PrimalInfeasibility = pres
		sol.DualInfeasibility = dres
		sol.PrimalSlack = 0.0
		sol.DualSlack = 0.0
		return
	}
	x = q.Copy()
	y = b.Copy()
	s = matrix.FloatZeros(cdim, 1)
	z = matrix.FloatZeros(cdim, 1)

	checkpnt.AddVerifiable("x", x)
	checkpnt.AddVerifiable("y", y)
	checkpnt.AddMatrixVar("s", s)
	checkpnt.AddMatrixVar("z", z)

	var ts, tz, nrms, nrmz float64

	if initvals == nil {
		// Factor
		//
		//     [ 0   A'  G' ]
		//     [ A   0   0  ].
		//     [ G   0  -I  ]
		//
		W = sets.NewFloatSet("d", "di", "v", "beta", "r", "rti")
		W.Set("d", matrix.FloatOnes(dims.At("l")[0], 1))
		W.Set("di", matrix.FloatOnes(dims.At("l")[0], 1))
		W.Set("beta", matrix.FloatOnes(len(dims.At("q")), 1))

		for _, n := range dims.At("q") {
			vm := matrix.FloatZeros(n, 1)
			vm.SetIndex(0, 1.0)
			W.Append("v", vm)
		}
		for _, n := range dims.At("s") {
			W.Append("r", matrix.FloatIdentity(n))
			W.Append("rti", matrix.FloatIdentity(n))
		}
		checkpnt.AddScaleVar(W)
		f, err = kktsolver(W)
		if err != nil {
			s := fmt.Sprintf("kkt error: %s", err)
			err = errors.New("3: Rank(A) < p or Rank([P; G; A]) < n : " + s)
			return
		}
		// Solve
		//
		//     [ P   A'  G' ]   [ x ]   [ -q ]
		//     [ A   0   0  ] * [ y ] = [  b ].
		//     [ G   0  -I  ]   [ z ]   [  h ]
		mCopy(q, x)
		x.Scal(-1.0)
		mCopy(b, y)
		blas.Copy(h, z)
		checkpnt.Check("00init", 1)
		err = f(x, y, z)
		if err != nil {
			s := fmt.Sprintf("kkt error: %s", err)
			err = errors.New("4: Rank(A) < p or Rank([P; G; A]) < n : " + s)
			return
		}
		blas.Copy(z, s)
		blas.ScalFloat(s, -1.0)
		checkpnt.Check("05init", 1)

		nrms = snrm2(s, dims, 0)
		ts, _ = maxStep(s, dims, 0, nil)
		//fmt.Printf("nrms = %.7f, ts = %.7f\n", nrms, ts)
		if ts >= -1e-8*math.Max(nrms, 1.0) {
			// a = 1.0 + ts
			a := 1.0 + ts
			is := make([]int, 0)
			// indexes s[:dims['l']]
			is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			// indexes s[indq[:-1]]
			is = append(is, indq[:len(indq)-1]...)
			ind := dims.Sum("l", "q")
			// indexes s[ind:ind+m*m:m+1] (diagonal)
			for _, m := range dims.At("s") {
				is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range is {
				s.SetIndex(k, a+s.GetIndex(k))
			}
		}

		nrmz = snrm2(z, dims, 0)
		tz, _ = maxStep(z, dims, 0, nil)
		//fmt.Printf("nrmz = %.7f, tz = %.7f\n", nrmz, tz)
		if tz >= -1e-8*math.Max(nrmz, 1.0) {
			a := 1.0 + tz
			is := make([]int, 0)
			is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			is = append(is, indq[:len(indq)-1]...)
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range is {
				z.SetIndex(k, a+z.GetIndex(k))
			}
		}

	} else {
		ix := initvals.At("x")[0]
		if ix != nil {
			mCopy(&matrixVar{ix}, x)
		} else {
			x.Scal(0.0)
		}

		is := initvals.At("s")[0]
		if is != nil {
			blas.Copy(is, s)
		} else {
			iset := make([]int, 0)
			iset = append(iset, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			iset = append(iset, indq[:len(indq)-1]...)
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				iset = append(iset, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range iset {
				s.SetIndex(k, 1.0)
			}
		}

		iy := initvals.At("y")[0]
		if iy != nil {
			mCopy(&matrixVar{iy}, y)
		} else {
			y.Scal(0.0)
		}

		iz := initvals.At("z")[0]
		if iz != nil {
			blas.Copy(iz, z)
		} else {
			iset := make([]int, 0)
			iset = append(iset, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			iset = append(iset, indq[:len(indq)-1]...)
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				iset = append(iset, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range iset {
				z.SetIndex(k, 1.0)
			}
		}
	}

	rx = q.Copy()
	ry = b.Copy()
	rz = matrix.FloatZeros(cdim, 1)
	dx = x.Copy()
	dy = y.Copy()
	dz = matrix.FloatZeros(cdim, 1)
	ds = matrix.FloatZeros(cdim, 1)
	lmbda = matrix.FloatZeros(cdim_diag, 1)
	lmbdasq = matrix.FloatZeros(cdim_diag, 1)
	sigs = matrix.FloatZeros(dims.Sum("s"), 1)
	sigz = matrix.FloatZeros(dims.Sum("s"), 1)

	checkpnt.AddVerifiable("rx", rx)
	checkpnt.AddVerifiable("ry", ry)
	checkpnt.AddVerifiable("dx", dx)
	checkpnt.AddVerifiable("dy", dy)
	//checkpnt.AddMatrixVar("rs", rs)
	checkpnt.AddMatrixVar("rz", rz)
	checkpnt.AddMatrixVar("ds", ds)
	checkpnt.AddMatrixVar("dz", dz)
	checkpnt.AddMatrixVar("lmbda", lmbda)
	checkpnt.AddMatrixVar("lmbdasq", lmbdasq)

	//var resx, resy, resz, step, sigma, mu, eta float64
	//var gap, pcost, dcost, relgap, pres, dres, f0 float64
	checkpnt.AddFloatVar("resx", &resx)
	checkpnt.AddFloatVar("resy", &resy)
	checkpnt.AddFloatVar("resz", &resz)
	checkpnt.AddFloatVar("step", &step)
	checkpnt.AddFloatVar("gap", &gap)
	checkpnt.AddFloatVar("dcost", &dcost)
	checkpnt.AddFloatVar("pcost", &pcost)
	checkpnt.AddFloatVar("dres", &dres)
	checkpnt.AddFloatVar("pres", &pres)
	checkpnt.AddFloatVar("relgap", &relgap)
	checkpnt.AddFloatVar("sigma", &sigma)

	var WS fVarClosure

	gap = sdot(s, z, dims, 0)
	for iter := 0; iter < maxIter+1; iter++ {
		checkpnt.MajorNext()
		checkpnt.Check("loopstart", 10)

		// f0 = (1/2)*x'*P*x + q'*x + r and  rx = P*x + q + A'*y + G'*z.
		mCopy(q, rx)
		fP(x, rx, 1.0, 1.0)
		f0 = 0.5 * (x.Dot(rx) + x.Dot(q))
		fA(y, rx, 1.0, 1.0, la.OptTrans)
		fG(&matrixVar{z}, rx, 1.0, 1.0, la.OptTrans)
		resx = math.Sqrt(rx.Dot(rx))

		// ry = A*x - b
		mCopy(b, ry)
		fA(x, ry, 1.0, -1.0, la.OptNoTrans)
		resy = math.Sqrt(ry.Dot(ry))

		// rz = s + G*x - h
		blas.Copy(s, rz)
		blas.AxpyFloat(h, rz, -1.0)
		fG(x, &matrixVar{rz}, 1.0, 1.0, la.OptNoTrans)
		resz = snrm2(rz, dims, 0)
		//fmt.Printf("resx: %.17f, resy: %.17f, resz: %.17f\n", resx, resy, resz)

		// Statistics for stopping criteria.

		// pcost = (1/2)*x'*P*x + q'*x
		// dcost = (1/2)*x'*P*x + q'*x + y'*(A*x-b) + z'*(G*x-h) '
		//       = (1/2)*x'*P*x + q'*x + y'*(A*x-b) + z'*(G*x-h+s) - z'*s
		//       = (1/2)*x'*P*x + q'*x + y'*ry + z'*rz - gap
		pcost = f0
		dcost = f0 + y.Dot(ry) + sdot(z, rz, dims, 0) - gap
		if pcost < 0.0 {
			relgap = gap / -pcost
		} else if dcost > 0.0 {
			relgap = gap / dcost
		} else {
			relgap = math.NaN()
		}
		pres = math.Max(resy/resy0, resz/resz0)
		dres = resx / resx0

		if solopts.ShowProgress {
			if iter == 0 {
				// show headers of something
				fmt.Printf("% 10s% 12s% 10s% 8s% 7s\n",
					"pcost", "dcost", "gap", "pres", "dres")
			}
			// show something
			fmt.Printf("%2d: % 8.4e % 8.4e % 4.0e% 7.0e% 7.0e\n",
				iter, pcost, dcost, gap, pres, dres)
		}
		checkpnt.Check("stoptest", 100)

		if pres <= feasTolerance && dres <= feasTolerance &&
			(gap <= absTolerance || (!math.IsNaN(relgap) && relgap <= relTolerance)) ||
			iter == maxIter {

			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				symm(s, m, ind)
				symm(z, m, ind)
				ind += m * m
			}
			ts, _ = maxStep(s, dims, 0, nil)
			tz, _ = maxStep(z, dims, 0, nil)
			if iter == maxIter {
				// terminated on max iterations.
				sol.Status = Unknown
				err = errors.New("Terminated (maximum iterations reached)")
				fmt.Printf("Terminated (maximum iterations reached)\n")
				return
			}
			// optimal solution found
			//fmt.Print("Optimal solution.\n")
			err = nil
			sol.Result = sets.NewFloatSet("x", "y", "s", "z")
			sol.Result.Set("x", x.Matrix())
			sol.Result.Set("y", y.Matrix())
			sol.Result.Set("s", s)
			sol.Result.Set("z", z)
			sol.Status = Optimal
			sol.Gap = gap
			sol.RelativeGap = relgap
			sol.PrimalObjective = pcost
			sol.DualObjective = dcost
			sol.PrimalInfeasibility = pres
			sol.DualInfeasibility = dres
			sol.PrimalSlack = -ts
			sol.DualSlack = -tz
			sol.PrimalResidualCert = math.NaN()
			sol.DualResidualCert = math.NaN()
			sol.Iterations = iter
			return
		}

		// Compute initial scaling W and scaled iterates:
		//
		//     W * z = W^{-T} * s = lambda.
		//
		// lmbdasq = lambda o lambda.
		if iter == 0 {
			W, err = computeScaling(s, z, lmbda, dims, 0)
			checkpnt.AddScaleVar(W)
		}
		ssqr(lmbdasq, lmbda, dims, 0)

		f3, err = kktsolver(W)
		if err != nil {
			if iter == 0 {
				s := fmt.Sprintf("kkt error: %s", err)
				err = errors.New("5: Rank(A) < p or Rank([P; A; G]) < n : " + s)
				return
			} else {
				ind := dims.Sum("l", "q")
				for _, m := range dims.At("s") {
					symm(s, m, ind)
					symm(z, m, ind)
					ind += m * m
				}
				ts, _ = maxStep(s, dims, 0, nil)
				tz, _ = maxStep(z, dims, 0, nil)
				// terminated (singular KKT matrix)
				fmt.Printf("Terminated (singular KKT matrix).\n")
				err = errors.New("Terminated (singular KKT matrix).")
				sol.Result = sets.NewFloatSet("x", "y", "s", "z")
				sol.Result.Set("x", x.Matrix())
				sol.Result.Set("y", y.Matrix())
				sol.Result.Set("s", s)
				sol.Result.Set("z", z)
				sol.Status = Unknown
				sol.RelativeGap = relgap
				sol.PrimalObjective = pcost
				sol.DualObjective = dcost
				sol.PrimalInfeasibility = pres
				sol.DualInfeasibility = dres
				sol.PrimalSlack = -ts
				sol.DualSlack = -tz
				sol.Iterations = iter
				return
			}
		}
		// f4_no_ir(x, y, z, s) solves
		//
		//     [ 0     ]   [ P  A'  G' ]   [ ux        ]   [ bx ]
		//     [ 0     ] + [ A  0   0  ] * [ uy        ] = [ by ]
		//     [ W'*us ]   [ G  0   0  ]   [ W^{-1}*uz ]   [ bz ]
		//
		//     lmbda o (uz + us) = bs.
		//
		// On entry, x, y, z, s contain bx, by, bz, bs.
		// On exit, they contain ux, uy, uz, us.

		f4_no_ir := func(x, y MatrixVariable, z, s *matrix.FloatMatrix) error {
			// Solve
			//
			//     [ P A' G'   ] [ ux        ]    [ bx                    ]
			//     [ A 0  0    ] [ uy        ] =  [ by                    ]
			//     [ G 0 -W'*W ] [ W^{-1}*uz ]    [ bz - W'*(lmbda o\ bs) ]
			//
			//     us = lmbda o\ bs - uz.
			//
			// On entry, x, y, z, s  contains bx, by, bz, bs.
			// On exit they contain x, y, z, s.

			minor := checkpnt.MinorTop()
			checkpnt.Check("f4_no_ir_start", minor)
			// s := lmbda o\ s
			//    = lmbda o\ bs
			sinv(s, lmbda, dims, 0)

			// z := z - W'*s
			//    = bz - W'*(lambda o\ bs)
			blas.Copy(s, ws3)
			scale(ws3, W, true, false)
			blas.AxpyFloat(ws3, z, -1.0)

			checkpnt.Check("f4_no_ir_f3", minor+50)
			err := f3(x, y, z)
			if err != nil {
				return err
			}
			checkpnt.Check("f4_no_ir_f3", minor+60)

			// s := s - z
			//    = lambda o\ bs - uz.
			blas.AxpyFloat(z, s, -1.0)
			checkpnt.Check("f4_no_ir_f3", minor+90)
			return nil
		}

		if iter == 0 {
			if refinement > 0 || solopts.Debug {
				WS.wx = q.Copy()
				WS.wy = y.Copy()
				WS.ws = matrix.FloatZeros(cdim, 1)
				WS.wz = matrix.FloatZeros(cdim, 1)
				checkpnt.AddVerifiable("wx", WS.wx)
				checkpnt.AddVerifiable("wy", WS.wy)
				checkpnt.AddMatrixVar("ws", WS.ws)
				checkpnt.AddMatrixVar("wz", WS.wz)
			}
			if refinement > 0 {
				WS.wx2 = q.Copy()
				WS.wy2 = y.Copy()
				WS.ws2 = matrix.FloatZeros(cdim, 1)
				WS.wz2 = matrix.FloatZeros(cdim, 1)
				checkpnt.AddVerifiable("wx2", WS.wx2)
				checkpnt.AddVerifiable("wy2", WS.wy2)
				checkpnt.AddMatrixVar("ws2", WS.ws2)
				checkpnt.AddMatrixVar("wz2", WS.wz2)
			}
		}

		f4 := func(x, y MatrixVariable, z, s *matrix.FloatMatrix) (err error) {
			minor := checkpnt.MinorTop()
			checkpnt.Check("f4start", minor)
			err = nil
			if refinement > 0 || solopts.Debug {
				mCopy(x, WS.wx)
				mCopy(y, WS.wy)
				blas.Copy(z, WS.wz)
				blas.Copy(s, WS.ws)
			}

			checkpnt.MinorPush(minor + 100)
			err = f4_no_ir(x, y, z, s)
			checkpnt.MinorPop()

			for i := 0; i < refinement; i++ {
				mCopy(WS.wx, WS.wx2)
				mCopy(WS.wy, WS.wy2)
				blas.Copy(WS.wz, WS.wz2)
				blas.Copy(WS.ws, WS.ws2)

				checkpnt.MinorPush(minor + (i+1)*300)
				res(x, y, z, s, WS.wx2, WS.wy2, WS.wz2, WS.ws2, W, lmbda)
				checkpnt.MinorPop()

				checkpnt.MinorPush(minor + (i+1)*500)
				f4_no_ir(WS.wx2, WS.wy2, WS.wz2, WS.ws2)
				checkpnt.MinorPop()

				WS.wx2.Axpy(x, 1.0)
				WS.wy2.Axpy(y, 1.0)
				blas.AxpyFloat(WS.wz2, z, 1.0)
				blas.AxpyFloat(WS.ws2, s, 1.0)
			}
			checkpnt.Check("f4end", minor+1500)
			return
		}

		//var mu, sigma, eta float64
		mu = gap / float64(dims.Sum("l", "s")+len(dims.At("q")))
		sigma, eta = 0.0, 0.0

		for i := 0; i < 2; i++ {
			// Solve
			//
			//     [ 0     ]   [ P  A' G' ]   [ dx        ]
			//     [ 0     ] + [ A  0  0  ] * [ dy        ] = -(1 - eta) * r
			//     [ W'*ds ]   [ G  0  0  ]   [ W^{-1}*dz ]
			//
			//     lmbda o (dz + ds) = -lmbda o lmbda + sigma*mu*e (i=0)
			//     lmbda o (dz + ds) = -lmbda o lmbda - dsa o dza
			//                         + sigma*mu*e (i=1) where dsa, dza
			//                         are the solution for i=0.

			minor_base := (i + 1) * 2000
			// ds = -lmbdasq + sigma * mu * e  (if i is 0)
			//    = -lmbdasq - dsa o dza + sigma * mu * e  (if i is 1),
			//    where ds, dz are solution for i is 0.
			blas.ScalFloat(ds, 0.0)
			if correction && i == 1 {
				blas.AxpyFloat(ws3, ds, -1.0)
			}
			blas.AxpyFloat(lmbdasq, ds, -1.0, &la.IOpt{"n", dims.Sum("l", "q")})
			ind := dims.At("l")[0]
			ds.Add(sigma*mu, matrix.MakeIndexSet(0, ind, 1)...)
			for _, m := range dims.At("q") {
				ds.SetIndex(ind, sigma*mu+ds.GetIndex(ind))
				ind += m
			}
			ind2 := ind
			for _, m := range dims.At("s") {
				blas.AxpyFloat(lmbdasq, ds, -1.0, &la.IOpt{"n", m}, &la.IOpt{"incy", m + 1},
					&la.IOpt{"offsetx", ind2}, &la.IOpt{"offsety", ind})
				ds.Add(sigma*mu, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
				ind2 += m
			}

			checkpnt.Check("00loop01", minor_base)

			// (dx, dy, dz) := -(1 - eta) * (rx, ry, rz)
			//blas.ScalFloat(dx, 0.0)
			//blas.AxpyFloat(rx, dx, -1.0+eta)
			dx.Scal(0.0)
			rx.Axpy(dx, -1.0+eta)
			dy.Scal(0.0)
			ry.Axpy(dy, -1.0+eta)
			blas.ScalFloat(dz, 0.0)
			blas.AxpyFloat(rz, dz, -1.0+eta)

			//fmt.Printf("== Calling f4 %d\n", i)
			//fmt.Printf("dx=\n%v\n", dx.ToString("%.17f"))
			//fmt.Printf("ds=\n%v\n", ds.ToString("%.17f"))
			//fmt.Printf("dz=\n%v\n", dz.ToString("%.17f"))
			//fmt.Printf("== Entering f4 %d\n", i)
			checkpnt.MinorPush(minor_base)
			err = f4(dx, dy, dz, ds)
			checkpnt.MinorPop()
			if err != nil {
				if iter == 0 {
					s := fmt.Sprintf("kkt error: %s", err)
					err = errors.New("6: Rank(A) < p or Rank([P; A; G]) < n : " + s)
					return
				} else {
					ind = dims.Sum("l", "q")
					for _, m := range dims.At("s") {
						symm(s, m, ind)
						symm(z, m, ind)
						ind += m * m
					}
					ts, _ = maxStep(s, dims, 0, nil)
					tz, _ = maxStep(z, dims, 0, nil)
					return
				}
			}

			dsdz := sdot(ds, dz, dims, 0)
			if correction && i == 0 {
				blas.Copy(ds, ws3)
				sprod(ws3, dz, dims, 0)
			}

			// Maximum step to boundary.
			//
			// If i is 1, also compute eigenvalue decomposition of the 's'
			// blocks in ds, dz.  The eigenvectors Qs, Qz are stored in
			// dsk, dzk.  The eigenvalues are stored in sigs, sigz.
			scale2(lmbda, ds, dims, 0, false)
			scale2(lmbda, dz, dims, 0, false)
			checkpnt.Check("maxstep", minor_base+1500)
			if i == 0 {
				ts, _ = maxStep(ds, dims, 0, nil)
				tz, _ = maxStep(dz, dims, 0, nil)
			} else {
				ts, _ = maxStep(ds, dims, 0, sigs)
				tz, _ = maxStep(dz, dims, 0, sigz)
			}
			t := maxvec([]float64{0.0, ts, tz})
			//fmt.Printf("== t=%.17f from %v\n", t, []float64{ts, tz})
			if t == 0.0 {
				step = 1.0
			} else {
				if i == 0 {
					step = math.Min(1.0, 1.0/t)
				} else {
					step = math.Min(1.0, STEP/t)
				}
			}
			if i == 0 {
				m := math.Max(0.0, 1.0-step+dsdz/gap*(step*step))
				sigma = math.Pow(math.Min(1.0, m), float64(EXPON))
				eta = 0.0
			}
			//fmt.Printf("== step=%.17f sigma=%.17f dsdz=%.17f\n", step, sigma, dsdz)

		}

		checkpnt.Check("updatexy", 8000)
		dx.Axpy(x, step)
		dy.Axpy(y, step)
		//fmt.Printf("x=\n%v\n", x.ConvertToString())
		//fmt.Printf("y=\n%v\n", y.ConvertToString())
		//fmt.Printf("ds=\n%v\n", ds.ConvertToString())
		//fmt.Printf("dz=\n%v\n", dz.ConvertToString())

		// We will now replace the 'l' and 'q' blocks of ds and dz with
		// the updated iterates in the current scaling.
		// We also replace the 's' blocks of ds and dz with the factors
		// Ls, Lz in a factorization Ls*Ls', Lz*Lz' of the updated variables
		// in the current scaling.

		// ds := e + step*ds for nonlinear, 'l' and 'q' blocks.
		// dz := e + step*dz for nonlinear, 'l' and 'q' blocks.
		blas.ScalFloat(ds, step, &la.IOpt{"n", dims.Sum("l", "q")})
		blas.ScalFloat(dz, step, &la.IOpt{"n", dims.Sum("l", "q")})
		ind := dims.At("l")[0]
		is := matrix.MakeIndexSet(0, ind, 1)
		ds.Add(1.0, is...)
		dz.Add(1.0, is...)
		for _, m := range dims.At("q") {
			ds.SetIndex(ind, 1.0+ds.GetIndex(ind))
			dz.SetIndex(ind, 1.0+dz.GetIndex(ind))
			ind += m
		}
		checkpnt.Check("updatedsdz", 8010)

		// ds := H(lambda)^{-1/2} * ds and dz := H(lambda)^{-1/2} * dz.
		//
		// This replaces the 'l' and 'q' components of ds and dz with the
		// updated variables in the current scaling.
		// The 's' components of ds and dz are replaced with
		//
		// diag(lmbda_k)^{1/2} * Qs * diag(lmbda_k)^{1/2}
		// diag(lmbda_k)^{1/2} * Qz * diag(lmbda_k)^{1/2}
		scale2(lmbda, ds, dims, 0, true)
		scale2(lmbda, dz, dims, 0, true)
		checkpnt.Check("scale2", 8030)

		// sigs := ( e + step*sigs ) ./ lambda for 's' blocks.
		// sigz := ( e + step*sigz ) ./ lambda for 's' blocks.
		blas.ScalFloat(sigs, step)
		blas.ScalFloat(sigz, step)
		sigs.Add(1.0)
		sigz.Add(1.0)
		sdimsum := dims.Sum("s")
		qdimsum := dims.Sum("l", "q")
		blas.TbsvFloat(lmbda, sigs, &la.IOpt{"n", sdimsum}, &la.IOpt{"k", 0},
			&la.IOpt{"lda", 1}, &la.IOpt{"offseta", qdimsum})
		blas.TbsvFloat(lmbda, sigz, &la.IOpt{"n", sdimsum}, &la.IOpt{"k", 0},
			&la.IOpt{"lda", 1}, &la.IOpt{"offseta", qdimsum})

		ind2 := qdimsum
		ind3 := 0
		sdims := dims.At("s")

		for k := 0; k < len(sdims); k++ {
			m := sdims[k]
			for i := 0; i < m; i++ {
				a := math.Sqrt(sigs.GetIndex(ind3 + i))
				blas.ScalFloat(ds, a, &la.IOpt{"offset", ind2 + m*i}, &la.IOpt{"n", m})
				a = math.Sqrt(sigz.GetIndex(ind3 + i))
				blas.ScalFloat(dz, a, &la.IOpt{"offset", ind2 + m*i}, &la.IOpt{"n", m})
			}
			ind2 += m * m
			ind3 += m
		}

		checkpnt.Check("updatescaling", 8050)
		err = updateScaling(W, lmbda, ds, dz)
		checkpnt.Check("afterscaling", 8060)

		// Unscale s, z, tau, kappa (unscaled variables are used only to
		// compute feasibility residuals).
		ind = dims.Sum("l", "q")
		ind2 = ind
		blas.Copy(lmbda, s, &la.IOpt{"n", ind})
		for _, m := range dims.At("s") {
			blas.ScalFloat(s, 0.0, &la.IOpt{"offset", ind2})
			blas.Copy(lmbda, s, &la.IOpt{"offsetx", ind}, &la.IOpt{"offsety", ind2},
				&la.IOpt{"n", m}, &la.IOpt{"incy", m + 1})
			ind += m
			ind2 += m * m
		}
		scale(s, W, true, false)

		ind = dims.Sum("l", "q")
		ind2 = ind
		blas.Copy(lmbda, z, &la.IOpt{"n", ind})
		for _, m := range dims.At("s") {
			blas.ScalFloat(z, 0.0, &la.IOpt{"offset", ind2})
			blas.Copy(lmbda, z, &la.IOpt{"offsetx", ind}, &la.IOpt{"offsety", ind2},
				&la.IOpt{"n", m}, &la.IOpt{"incy", m + 1})
			ind += m
			ind2 += m * m
		}
		scale(z, W, false, true)

		gap = blas.DotFloat(lmbda, lmbda)
		checkpnt.Check("eol", 8900)
		//fmt.Printf("== gap = %.17f\n", gap)
	}
	return
}
Exemple #3
0
func conelp_solver(c MatrixVariable, G MatrixVarG, h *matrix.FloatMatrix,
	A MatrixVarA, b MatrixVariable, dims *sets.DimensionSet, kktsolver KKTConeSolverVar,
	solopts *SolverOptions, primalstart, dualstart *sets.FloatMatrixSet) (sol *Solution, err error) {

	err = nil
	const EXPON = 3
	const STEP = 0.99

	sol = &Solution{Unknown,
		nil,
		0.0, 0.0, 0.0, 0.0, 0.0,
		0.0, 0.0, 0.0, 0.0, 0.0, 0}

	var refinement int

	if solopts.Refinement > 0 {
		refinement = solopts.Refinement
	} else {
		refinement = 0
		if len(dims.At("q")) > 0 || len(dims.At("s")) > 0 {
			refinement = 1
		}
	}
	feasTolerance := FEASTOL
	absTolerance := ABSTOL
	relTolerance := RELTOL
	maxIter := MAXITERS
	if solopts.FeasTol > 0.0 {
		feasTolerance = solopts.FeasTol
	}
	if solopts.AbsTol > 0.0 {
		absTolerance = solopts.AbsTol
	}
	if solopts.RelTol > 0.0 {
		relTolerance = solopts.RelTol
	}
	if solopts.MaxIter > 0 {
		maxIter = solopts.MaxIter
	}
	if err = checkConeLpDimensions(dims); err != nil {
		return
	}

	cdim := dims.Sum("l", "q") + dims.SumSquared("s")
	//cdim_pckd := dims.Sum("l", "q") + dims.SumPacked("s")
	cdim_diag := dims.Sum("l", "q", "s")

	if h.Rows() != cdim {
		err = errors.New(fmt.Sprintf("'h' must be float matrix of size (%d,1)", cdim))
		return
	}

	// Data for kth 'q' constraint are found in rows indq[k]:indq[k+1] of G.
	indq := make([]int, 0)
	indq = append(indq, dims.At("l")[0])
	for _, k := range dims.At("q") {
		indq = append(indq, indq[len(indq)-1]+k)
	}

	// Data for kth 's' constraint are found in rows inds[k]:inds[k+1] of G.
	inds := make([]int, 0)
	inds = append(inds, indq[len(indq)-1])
	for _, k := range dims.At("s") {
		inds = append(inds, inds[len(inds)-1]+k*k)
	}

	Gf := func(x, y MatrixVariable, alpha, beta float64, trans la.Option) error {
		return G.Gf(x, y, alpha, beta, trans)
	}

	Af := func(x, y MatrixVariable, alpha, beta float64, trans la.Option) error {
		return A.Af(x, y, alpha, beta, trans)
	}

	// kktsolver(W) returns a routine for solving 3x3 block KKT system
	//
	//     [ 0   A'  G'*W^{-1} ] [ ux ]   [ bx ]
	//     [ A   0   0         ] [ uy ] = [ by ].
	//     [ G   0   -W'       ] [ uz ]   [ bz ]

	if kktsolver == nil {
		err = errors.New("nil kktsolver not allowed.")
		return
	}

	// res() evaluates residual in 5x5 block KKT system
	//
	//     [ vx   ]    [ 0         ]   [ 0   A'  G'  c ] [ ux        ]
	//     [ vy   ]    [ 0         ]   [-A   0   0   b ] [ uy        ]
	//     [ vz   ] += [ W'*us     ] - [-G   0   0   h ] [ W^{-1}*uz ]
	//     [ vtau ]    [ dg*ukappa ]   [-c' -b' -h'  0 ] [ utau/dg   ]
	//
	//           vs += lmbda o (dz + ds)
	//       vkappa += lmbdg * (dtau + dkappa).
	ws3 := matrix.FloatZeros(cdim, 1)
	wz3 := matrix.FloatZeros(cdim, 1)
	checkpnt.AddMatrixVar("ws3", ws3)
	checkpnt.AddMatrixVar("wz3", wz3)

	//
	res := func(ux, uy MatrixVariable, uz, utau, us, ukappa *matrix.FloatMatrix,
		vx, vy MatrixVariable, vz, vtau, vs, vkappa *matrix.FloatMatrix,
		W *sets.FloatMatrixSet, dg float64, lmbda *matrix.FloatMatrix) (err error) {

		err = nil
		// vx := vx - A'*uy - G'*W^{-1}*uz - c*utau/dg
		Af(uy, vx, -1.0, 1.0, la.OptTrans)
		//fmt.Printf("post-Af vx=\n%v\n", vx)
		blas.Copy(uz, wz3)
		scale(wz3, W, false, true)
		Gf(&matrixVar{wz3}, vx, -1.0, 1.0, la.OptTrans)
		//blas.AxpyFloat(c, vx, -utau.Float()/dg)
		c.Axpy(vx, -utau.Float()/dg)

		// vy := vy + A*ux - b*utau/dg
		Af(ux, vy, 1.0, 1.0, la.OptNoTrans)
		//blas.AxpyFloat(b, vy, -utau.Float()/dg)
		b.Axpy(vy, -utau.Float()/dg)

		// vz := vz + G*ux - h*utau/dg + W'*us
		Gf(ux, &matrixVar{vz}, 1.0, 1.0, la.OptNoTrans)
		blas.AxpyFloat(h, vz, -utau.Float()/dg)
		blas.Copy(us, ws3)
		scale(ws3, W, true, false)
		blas.AxpyFloat(ws3, vz, 1.0)

		// vtau := vtau + c'*ux + b'*uy + h'*W^{-1}*uz + dg*ukappa
		var vtauplus float64 = dg*ukappa.Float() + c.Dot(ux) +
			b.Dot(uy) + sdot(h, wz3, dims, 0)
		vtau.SetValue(vtau.Float() + vtauplus)

		// vs := vs + lmbda o (uz + us)
		blas.Copy(us, ws3)
		blas.AxpyFloat(uz, ws3, 1.0)
		sprod(ws3, lmbda, dims, 0, &la.SOpt{"diag", "D"})
		blas.AxpyFloat(ws3, vs, 1.0)

		// vkappa += vkappa + lmbdag * (utau + ukappa)
		lscale := lmbda.GetIndex(-1)
		var vkplus float64 = lscale * (utau.Float() + ukappa.Float())
		vkappa.SetValue(vkappa.Float() + vkplus)
		return
	}

	resx0 := math.Max(1.0, math.Sqrt(c.Dot(c)))
	resy0 := math.Max(1.0, math.Sqrt(b.Dot(b)))
	resz0 := math.Max(1.0, snrm2(h, dims, 0))

	// select initial points

	//fmt.Printf("** initial resx0=%.4f, resy0=%.4f, resz0=%.4f \n", resx0, resy0, resz0)

	x := c.Copy()
	//blas.ScalFloat(x, 0.0)
	x.Scal(0.0)
	y := b.Copy()
	//blas.ScalFloat(y, 0.0)
	y.Scal(0.0)

	s := matrix.FloatZeros(cdim, 1)
	z := matrix.FloatZeros(cdim, 1)
	dx := c.Copy()
	dy := b.Copy()
	ds := matrix.FloatZeros(cdim, 1)
	dz := matrix.FloatZeros(cdim, 1)
	// these are singleton matrix
	dkappa := matrix.FloatValue(0.0)
	dtau := matrix.FloatValue(0.0)

	checkpnt.AddVerifiable("x", x)
	checkpnt.AddMatrixVar("s", s)
	checkpnt.AddMatrixVar("z", z)
	checkpnt.AddVerifiable("dx", dx)
	checkpnt.AddMatrixVar("ds", ds)
	checkpnt.AddMatrixVar("dz", dz)
	checkpnt.Check("00init", 1)

	var W *sets.FloatMatrixSet
	var f KKTFuncVar
	if primalstart == nil || dualstart == nil {
		// Factor
		//
		//     [ 0   A'  G' ]
		//     [ A   0   0  ].
		//     [ G   0  -I  ]
		//
		W = sets.NewFloatSet("d", "di", "v", "beta", "r", "rti")
		dd := dims.At("l")[0]
		mat := matrix.FloatOnes(dd, 1)
		W.Set("d", mat)
		mat = matrix.FloatOnes(dd, 1)
		W.Set("di", mat)
		dq := len(dims.At("q"))
		W.Set("beta", matrix.FloatOnes(dq, 1))

		for _, n := range dims.At("q") {
			vm := matrix.FloatZeros(n, 1)
			vm.SetIndex(0, 1.0)
			W.Append("v", vm)
		}
		for _, n := range dims.At("s") {
			W.Append("r", matrix.FloatIdentity(n))
			W.Append("rti", matrix.FloatIdentity(n))
		}
		f, err = kktsolver(W)
		if err != nil {
			fmt.Printf("kktsolver error: %s\n", err)
			return
		}
		checkpnt.AddScaleVar(W)
	}

	checkpnt.Check("05init", 5)
	if primalstart == nil {
		// minimize    || G * x - h ||^2
		// subject to  A * x = b
		//
		// by solving
		//
		//     [ 0   A'  G' ]   [ x  ]   [ 0 ]
		//     [ A   0   0  ] * [ dy ] = [ b ].
		//     [ G   0  -I  ]   [ -s ]   [ h ]
		//blas.ScalFloat(x, 0.0)
		//blas.CopyFloat(b, dy)
		checkpnt.MinorPush(5)
		x.Scal(0.0)
		mCopy(b, dy)
		blas.CopyFloat(h, s)

		err = f(x, dy, s)
		if err != nil {
			fmt.Printf("f(x,dy,s): %s\n", err)
			return
		}
		blas.ScalFloat(s, -1.0)
		//fmt.Printf("initial s=\n%v\n", s.ToString("%.5f"))
		checkpnt.MinorPop()
	} else {
		mCopy(&matrixVar{primalstart.At("x")[0]}, x)
		blas.Copy(primalstart.At("s")[0], s)
	}

	// ts = min{ t | s + t*e >= 0 }
	ts, _ := maxStep(s, dims, 0, nil)
	if ts >= 0 && primalstart != nil {
		err = errors.New("initial s is not positive")
		return
	}
	//fmt.Printf("initial ts=%.5f\n", ts)
	checkpnt.Check("10init", 10)

	if dualstart == nil {
		// minimize   || z ||^2
		// subject to G'*z + A'*y + c = 0
		//
		// by solving
		//
		//     [ 0   A'  G' ] [ dx ]   [ -c ]
		//     [ A   0   0  ] [ y  ] = [  0 ].
		//     [ G   0  -I  ] [ z  ]   [  0 ]
		//blas.Copy(c, dx)
		//blas.ScalFloat(dx, -1.0)
		//blas.ScalFloat(y, 0.0)
		checkpnt.MinorPush(10)
		mCopy(c, dx)
		dx.Scal(-1.0)
		y.Scal(0.0)
		blas.ScalFloat(z, 0.0)
		err = f(dx, y, z)
		if err != nil {
			fmt.Printf("f(dx,y,z): %s\n", err)
			return
		}
		//fmt.Printf("initial z=\n%v\n", z.ToString("%.5f"))
		checkpnt.MinorPop()
	} else {
		if len(dualstart.At("y")) > 0 {
			mCopy(&matrixVar{dualstart.At("y")[0]}, y)
		}
		blas.Copy(dualstart.At("z")[0], z)
	}

	// ts = min{ t | z + t*e >= 0 }
	tz, _ := maxStep(z, dims, 0, nil)
	if tz >= 0 && dualstart != nil {
		err = errors.New("initial z is not positive")
		return
	}
	//fmt.Printf("initial tz=%.5f\n", tz)

	nrms := snrm2(s, dims, 0)
	nrmz := snrm2(z, dims, 0)

	gap := 0.0
	pcost := 0.0
	dcost := 0.0
	relgap := 0.0

	checkpnt.Check("20init", 0)

	if primalstart == nil && dualstart == nil {
		gap = sdot(s, z, dims, 0)
		pcost = c.Dot(x)
		dcost = -b.Dot(y) - sdot(h, z, dims, 0)
		if pcost < 0.0 {
			relgap = gap / -pcost
		} else if dcost > 0.0 {
			relgap = gap / dcost
		} else {
			relgap = math.NaN()
		}
		if ts <= 0 && tz < 0 &&
			(gap <= absTolerance || (!math.IsNaN(relgap) && relgap <= relTolerance)) {
			// Constructed initial points happen to be feasible and optimal

			ind := dims.At("l")[0] + dims.Sum("q")
			for _, m := range dims.At("s") {
				symm(s, m, ind)
				symm(z, m, ind)
				ind += m * m
			}

			// rx = A'*y + G'*z + c
			rx := c.Copy()
			Af(y, rx, 1.0, 1.0, la.OptTrans)
			Gf(&matrixVar{z}, rx, 1.0, 1.0, la.OptTrans)
			resx := math.Sqrt(rx.Dot(rx))
			// ry = b - A*x
			ry := b.Copy()
			Af(x, ry, -1.0, -1.0, la.OptNoTrans)
			resy := math.Sqrt(ry.Dot(ry))
			// rz = s + G*x - h
			rz := matrix.FloatZeros(cdim, 1)

			Gf(x, &matrixVar{rz}, 1.0, 0.0, la.OptNoTrans)
			blas.AxpyFloat(s, rz, 1.0)
			blas.AxpyFloat(h, rz, -1.0)
			resz := snrm2(rz, dims, 0)

			pres := math.Max(resy/resy0, resz/resz0)
			dres := resx / resx0
			cx := c.Dot(x)
			by := b.Dot(y)
			hz := sdot(h, z, dims, 0)

			//sol.X = x; sol.Y = y; sol.S = s; sol.Z = z
			sol.Result = sets.NewFloatSet("x", "y", "s", "x")
			sol.Result.Append("x", x.Matrix())
			sol.Result.Append("y", y.Matrix())
			sol.Result.Append("s", s)
			sol.Result.Append("z", z)
			sol.Status = Optimal
			sol.Gap = gap
			sol.RelativeGap = relgap
			sol.PrimalObjective = cx
			sol.DualObjective = -(by + hz)
			sol.PrimalInfeasibility = pres
			sol.DualInfeasibility = dres
			sol.PrimalSlack = -ts
			sol.DualSlack = -tz

			return
		}

		if ts >= -1e-8*math.Max(nrms, 1.0) {
			a := 1.0 + ts
			is := make([]int, 0)
			// indexes s[:dims['l']]
			if dims.At("l")[0] > 0 {
				is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			}
			// indexes s[indq[:-1]]
			if len(indq) > 1 {
				is = append(is, indq[:len(indq)-1]...)
			}
			// indexes s[ind:ind+m*m:m+1] (diagonal)
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range is {
				s.SetIndex(k, a+s.GetIndex(k))
			}
		}

		if tz >= -1e-8*math.Max(nrmz, 1.0) {
			a := 1.0 + tz
			is := make([]int, 0)
			// indexes z[:dims['l']]
			if dims.At("l")[0] > 0 {
				is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			}
			// indexes z[indq[:-1]]
			if len(indq) > 1 {
				is = append(is, indq[:len(indq)-1]...)
			}
			// indexes z[ind:ind+m*m:m+1] (diagonal)
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range is {
				z.SetIndex(k, a+z.GetIndex(k))
			}
		}
	} else if primalstart == nil && dualstart != nil {
		if ts >= -1e-8*math.Max(nrms, 1.0) {
			a := 1.0 + ts
			is := make([]int, 0)
			if dims.At("l")[0] > 0 {
				is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			}
			if len(indq) > 1 {
				is = append(is, indq[:len(indq)-1]...)
			}
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range is {
				s.SetIndex(k, a+s.GetIndex(k))
			}
		}
	} else if primalstart != nil && dualstart == nil {
		if tz >= -1e-8*math.Max(nrmz, 1.0) {
			a := 1.0 + tz
			is := make([]int, 0)
			if dims.At("l")[0] > 0 {
				is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
			}
			if len(indq) > 1 {
				is = append(is, indq[:len(indq)-1]...)
			}
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
				ind += m * m
			}
			for _, k := range is {
				z.SetIndex(k, a+z.GetIndex(k))
			}
		}
	}

	tau := matrix.FloatValue(1.0)
	kappa := matrix.FloatValue(1.0)
	wkappa3 := matrix.FloatValue(0.0)

	rx := c.Copy()
	hrx := c.Copy()
	ry := b.Copy()
	hry := b.Copy()
	rz := matrix.FloatZeros(cdim, 1)
	hrz := matrix.FloatZeros(cdim, 1)
	sigs := matrix.FloatZeros(dims.Sum("s"), 1)
	sigz := matrix.FloatZeros(dims.Sum("s"), 1)
	lmbda := matrix.FloatZeros(cdim_diag+1, 1)
	lmbdasq := matrix.FloatZeros(cdim_diag+1, 1)

	gap = sdot(s, z, dims, 0)

	var x1, y1 MatrixVariable
	var z1 *matrix.FloatMatrix
	var dg, dgi float64
	var th *matrix.FloatMatrix
	var WS fVarClosure
	var f3 KKTFuncVar
	var cx, by, hz, rt float64
	var hresx, resx, hresy, resy, hresz, resz float64
	var dres, pres, dinfres, pinfres float64

	// check point variables
	checkpnt.AddMatrixVar("lmbda", lmbda)
	checkpnt.AddMatrixVar("lmbdasq", lmbdasq)
	checkpnt.AddVerifiable("rx", rx)
	checkpnt.AddVerifiable("ry", ry)
	checkpnt.AddMatrixVar("rz", rz)
	checkpnt.AddFloatVar("resx", &resx)
	checkpnt.AddFloatVar("resy", &resy)
	checkpnt.AddFloatVar("resz", &resz)
	checkpnt.AddFloatVar("hresx", &hresx)
	checkpnt.AddFloatVar("hresy", &hresy)
	checkpnt.AddFloatVar("hresz", &hresz)
	checkpnt.AddFloatVar("cx", &cx)
	checkpnt.AddFloatVar("by", &by)
	checkpnt.AddFloatVar("hz", &hz)
	checkpnt.AddFloatVar("gap", &gap)
	checkpnt.AddFloatVar("pres", &pres)
	checkpnt.AddFloatVar("dres", &dres)

	for iter := 0; iter < maxIter+1; iter++ {
		checkpnt.MajorNext()
		checkpnt.Check("loop-start", 100)

		// hrx = -A'*y - G'*z
		Af(y, hrx, -1.0, 0.0, la.OptTrans)
		Gf(&matrixVar{z}, hrx, -1.0, 1.0, la.OptTrans)
		hresx = math.Sqrt(hrx.Dot(hrx))

		// rx = hrx - c*tau
		//    = -A'*y - G'*z - c*tau
		mCopy(hrx, rx)
		c.Axpy(rx, -tau.Float())
		resx = math.Sqrt(rx.Dot(rx)) / tau.Float()

		// hry = A*x
		Af(x, hry, 1.0, 0.0, la.OptNoTrans)
		hresy = math.Sqrt(hry.Dot(hry))

		// ry = hry - b*tau
		//    = A*x - b*tau
		mCopy(hry, ry)
		b.Axpy(ry, -tau.Float())
		resy = math.Sqrt(ry.Dot(ry)) / tau.Float()

		// hrz = s + G*x
		Gf(x, &matrixVar{hrz}, 1.0, 0.0, la.OptNoTrans)
		blas.AxpyFloat(s, hrz, 1.0)
		hresz = snrm2(hrz, dims, 0)

		// rz = hrz - h*tau
		//    = s + G*x - h*tau
		blas.ScalFloat(rz, 0.0)
		blas.AxpyFloat(hrz, rz, 1.0)
		blas.AxpyFloat(h, rz, -tau.Float())
		resz = snrm2(rz, dims, 0) / tau.Float()

		// rt = kappa + c'*x + b'*y + h'*z '
		cx = c.Dot(x)
		by = b.Dot(y)
		hz = sdot(h, z, dims, 0)
		rt = kappa.Float() + cx + by + hz

		// Statistics for stopping criteria
		pcost = cx / tau.Float()
		dcost = -(by + hz) / tau.Float()

		if pcost < 0.0 {
			relgap = gap / -pcost
		} else if dcost > 0.0 {
			relgap = gap / dcost
		} else {
			relgap = math.NaN()
		}

		pres = math.Max(resy/resy0, resz/resz0)
		dres = resx / resx0
		pinfres = math.NaN()
		if hz+by < 0.0 {
			pinfres = hresx / resx0 / (-hz - by)
		}
		dinfres = math.NaN()
		if cx < 0.0 {
			dinfres = math.Max(hresy/resy0, hresz/resz0) / (-cx)
		}

		if solopts.ShowProgress {
			if iter == 0 {
				// show headers of something
				fmt.Printf("% 10s% 12s% 10s% 8s% 7s % 5s\n",
					"pcost", "dcost", "gap", "pres", "dres", "k/t")
			}
			// show something
			fmt.Printf("%2d: % 8.4e % 8.4e % 4.0e% 7.0e% 7.0e% 7.0e\n",
				iter, pcost, dcost, gap, pres, dres, kappa.GetIndex(0)/tau.GetIndex(0))
		}

		checkpnt.Check("isready", 200)

		if (pres <= feasTolerance && dres <= feasTolerance &&
			(gap <= absTolerance || (!math.IsNaN(relgap) && relgap <= relTolerance))) ||
			iter == maxIter {
			// done
			x.Scal(1.0 / tau.Float())
			y.Scal(1.0 / tau.Float())
			blas.ScalFloat(s, 1.0/tau.Float())
			blas.ScalFloat(z, 1.0/tau.Float())
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				symm(s, m, ind)
				symm(z, m, ind)
				ind += m * m
			}
			ts, _ = maxStep(s, dims, 0, nil)
			tz, _ = maxStep(z, dims, 0, nil)
			if iter == maxIter {
				// MaxIterations exceeded
				if solopts.ShowProgress {
					fmt.Printf("No solution. Max iterations exceeded\n")
				}
				err = errors.New("No solution. Max iterations exceeded")
				//sol.X = x; sol.Y = y; sol.S = s; sol.Z = z
				sol.Result = sets.NewFloatSet("x", "y", "s", "x")
				sol.Result.Append("x", x.Matrix())
				sol.Result.Append("y", y.Matrix())
				sol.Result.Append("s", s)
				sol.Result.Append("z", z)
				sol.Status = Unknown
				sol.Gap = gap
				sol.RelativeGap = relgap
				sol.PrimalObjective = pcost
				sol.DualObjective = dcost
				sol.PrimalInfeasibility = pres
				sol.DualInfeasibility = dres
				sol.PrimalSlack = -ts
				sol.DualSlack = -tz
				sol.PrimalResidualCert = pinfres
				sol.DualResidualCert = dinfres
				sol.Iterations = iter
				return
			} else {
				// Optimal
				if solopts.ShowProgress {
					fmt.Printf("Optimal solution.\n")
				}
				err = nil
				//sol.X = x; sol.Y = y; sol.S = s; sol.Z = z
				sol.Result = sets.NewFloatSet("x", "y", "s", "x")
				sol.Result.Append("x", x.Matrix())
				sol.Result.Append("y", y.Matrix())
				sol.Result.Append("s", s)
				sol.Result.Append("z", z)
				sol.Status = Optimal
				sol.Gap = gap
				sol.RelativeGap = relgap
				sol.PrimalObjective = pcost
				sol.DualObjective = dcost
				sol.PrimalInfeasibility = pres
				sol.DualInfeasibility = dres
				sol.PrimalSlack = -ts
				sol.DualSlack = -tz
				sol.PrimalResidualCert = math.NaN()
				sol.DualResidualCert = math.NaN()
				sol.Iterations = iter
				return
			}
		} else if !math.IsNaN(pinfres) && pinfres <= feasTolerance {
			// Primal Infeasible
			if solopts.ShowProgress {
				fmt.Printf("Primal infeasible.\n")
			}
			err = errors.New("Primal infeasible")
			y.Scal(1.0 / (-hz - by))
			blas.ScalFloat(z, 1.0/(-hz-by))
			//sol.X = nil; sol.Y = nil; sol.S = nil; sol.Z = nil
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				symm(z, m, ind)
				ind += m * m
			}
			tz, _ = maxStep(z, dims, 0, nil)
			sol.Status = PrimalInfeasible
			sol.Result = sets.NewFloatSet("x", "y", "s", "x")
			sol.Result.Append("x", nil)
			sol.Result.Append("y", nil)
			sol.Result.Append("s", nil)
			sol.Result.Append("z", nil)
			sol.Gap = math.NaN()
			sol.RelativeGap = math.NaN()
			sol.PrimalObjective = math.NaN()
			sol.DualObjective = 1.0
			sol.PrimalInfeasibility = math.NaN()
			sol.DualInfeasibility = math.NaN()
			sol.PrimalSlack = math.NaN()
			sol.DualSlack = -tz
			sol.PrimalResidualCert = pinfres
			sol.DualResidualCert = math.NaN()
			sol.Iterations = iter
			return
		} else if !math.IsNaN(dinfres) && dinfres <= feasTolerance {
			// Dual Infeasible
			if solopts.ShowProgress {
				fmt.Printf("Dual infeasible.\n")
			}
			err = errors.New("Primal infeasible")
			x.Scal(1.0 / (-cx))
			blas.ScalFloat(s, 1.0/(-cx))
			//sol.X = nil; sol.Y = nil; sol.S = nil; sol.Z = nil
			ind := dims.Sum("l", "q")
			for _, m := range dims.At("s") {
				symm(s, m, ind)
				ind += m * m
			}
			ts, _ = maxStep(s, dims, 0, nil)
			sol.Status = PrimalInfeasible
			sol.Result = sets.NewFloatSet("x", "y", "s", "x")
			sol.Result.Append("x", nil)
			sol.Result.Append("y", nil)
			sol.Result.Append("s", nil)
			sol.Result.Append("z", nil)
			sol.Gap = math.NaN()
			sol.RelativeGap = math.NaN()
			sol.PrimalObjective = 1.0
			sol.DualObjective = math.NaN()
			sol.PrimalInfeasibility = math.NaN()
			sol.DualInfeasibility = math.NaN()
			sol.PrimalSlack = -ts
			sol.DualSlack = math.NaN()
			sol.PrimalResidualCert = math.NaN()
			sol.DualResidualCert = dinfres
			sol.Iterations = iter
			return
		}

		// Compute initial scaling W:
		//
		//     W * z = W^{-T} * s = lambda
		//     dg * tau = 1/dg * kappa = lambdag.
		if iter == 0 {
			//fmt.Printf("compute scaling: lmbda=\n%v\n", lmbda.ToString("%.17f"))
			//fmt.Printf("s=\n%v\n", s.ToString("%.17f"))
			//fmt.Printf("z=\n%v\n", z.ToString("%.17f"))
			W, err = computeScaling(s, z, lmbda, dims, 0)
			checkpnt.AddScaleVar(W)

			//     dg = sqrt( kappa / tau )
			//     dgi = sqrt( tau / kappa )
			//     lambda_g = sqrt( tau * kappa )
			//
			// lambda_g is stored in the last position of lmbda.

			dg = math.Sqrt(kappa.Float() / tau.Float())
			dgi = math.Sqrt(float64(tau.Float() / kappa.Float()))
			lmbda.SetIndex(-1, math.Sqrt(float64(tau.Float()*kappa.Float())))
			//fmt.Printf("lmbda=\n%v\n", lmbda.ToString("%.17f"))
			//W.Print()
			checkpnt.Check("compute_scaling", 300)
		}
		// lmbdasq := lmbda o lmbda
		ssqr(lmbdasq, lmbda, dims, 0)
		lmbdasq.SetIndex(-1, lmbda.GetIndex(-1)*lmbda.GetIndex(-1))

		// f3(x, y, z) solves
		//
		//     [ 0  A'  G'   ] [ ux        ]   [ bx ]
		//     [ A  0   0    ] [ uy        ] = [ by ].
		//     [ G  0  -W'*W ] [ W^{-1}*uz ]   [ bz ]
		//
		// On entry, x, y, z contain bx, by, bz.
		// On exit, they contain ux, uy, uz.
		//
		// Also solve
		//
		//     [ 0   A'  G'    ] [ x1        ]          [ c ]
		//     [-A   0   0     ]*[ y1        ] = -dgi * [ b ].
		//     [-G   0   W'*W  ] [ W^{-1}*z1 ]          [ h ]

		f3, err = kktsolver(W)
		if err != nil {
			fmt.Printf("kktsolver error=%v\n", err)
			return
		}
		if iter == 0 {
			x1 = c.Copy()
			y1 = b.Copy()
			z1 = matrix.FloatZeros(cdim, 1)
			checkpnt.AddVerifiable("x1", x1)
			checkpnt.AddMatrixVar("z1", z1)
		}
		mCopy(c, x1)
		x1.Scal(-1.0)
		mCopy(b, y1)
		blas.Copy(h, z1)
		err = f3(x1, y1, z1)
		//fmt.Printf("f3 result: x1=\n%v\nf3 result: z1=\n%v\n", x1, z1)
		x1.Scal(dgi)
		y1.Scal(dgi)
		blas.ScalFloat(z1, dgi)

		if err != nil {
			if iter == 0 && primalstart != nil && dualstart != nil {
				err = errors.New("Rank(A) < p or Rank([G; A]) < n")
				return
			} else {
				t_ := 1.0 / tau.Float()
				x.Scal(t_)
				y.Scal(t_)
				blas.ScalFloat(s, t_)
				blas.ScalFloat(z, t_)
				ind := dims.Sum("l", "q")
				for _, m := range dims.At("s") {
					symm(s, m, ind)
					symm(z, m, ind)
					ind += m * m
				}
				ts, _ = maxStep(s, dims, 0, nil)
				tz, _ = maxStep(z, dims, 0, nil)
				err = errors.New("Terminated (singular KKT matrix).")
				//sol.X = x; sol.Y = y; sol.S = s; sol.Z = z
				sol.Result = sets.NewFloatSet("x", "y", "s", "x")
				sol.Result.Append("x", x.Matrix())
				sol.Result.Append("y", y.Matrix())
				sol.Result.Append("s", s)
				sol.Result.Append("z", z)
				sol.Status = Unknown
				sol.RelativeGap = relgap
				sol.PrimalObjective = pcost
				sol.DualObjective = dcost
				sol.PrimalInfeasibility = pres
				sol.DualInfeasibility = dres
				sol.PrimalSlack = -ts
				sol.DualSlack = -tz
				sol.Iterations = iter
				return
			}
		}

		// f6_no_ir(x, y, z, tau, s, kappa) solves
		//
		//    [ 0         ]   [  0   A'  G'  c ] [ ux        ]    [ bx   ]
		//    [ 0         ]   [ -A   0   0   b ] [ uy        ]    [ by   ]
		//    [ W'*us     ] - [ -G   0   0   h ] [ W^{-1}*uz ] = -[ bz   ]
		//    [ dg*ukappa ]   [ -c' -b' -h'  0 ] [ utau/dg   ]    [ btau ]
		//
		//    lmbda o (uz + us) = -bs
		//    lmbdag * (utau + ukappa) = -bkappa.
		//
		// On entry, x, y, z, tau, s, kappa contain bx, by, bz, btau,
		// bkappa.  On exit, they contain ux, uy, uz, utau, ukappa.

		// th = W^{-T} * h
		if iter == 0 {
			th = matrix.FloatZeros(cdim, 1)
			checkpnt.AddMatrixVar("th", th)
		}

		blas.Copy(h, th)
		scale(th, W, true, true)

		f6_no_ir := func(x, y MatrixVariable, z, tau, s, kappa *matrix.FloatMatrix) (err error) {
			// Solve
			//
			// [  0   A'  G'    0   ] [ ux        ]
			// [ -A   0   0     b   ] [ uy        ]
			// [ -G   0   W'*W  h   ] [ W^{-1}*uz ]
			// [ -c' -b' -h'    k/t ] [ utau/dg   ]
			//
			//   [ bx                    ]
			//   [ by                    ]
			// = [ bz - W'*(lmbda o\ bs) ]
			//   [ btau - bkappa/tau     ]
			//
			// us = -lmbda o\ bs - uz
			// ukappa = -bkappa/lmbdag - utau.

			// First solve
			//
			// [ 0  A' G'   ] [ ux        ]   [  bx                    ]
			// [ A  0  0    ] [ uy        ] = [ -by                    ]
			// [ G  0 -W'*W ] [ W^{-1}*uz ]   [ -bz + W'*(lmbda o\ bs) ]

			minor := checkpnt.MinorTop()
			err = nil
			// y := -y = -by
			y.Scal(-1.0)

			// s := -lmbda o\ s = -lmbda o\ bs
			err = sinv(s, lmbda, dims, 0)
			blas.ScalFloat(s, -1.0)

			// z := -(z + W'*s) = -bz + W'*(lambda o\ bs)
			blas.Copy(s, ws3)
			checkpnt.Check("prescale", minor+5)
			checkpnt.MinorPush(minor + 5)
			err = scale(ws3, W, true, false)
			checkpnt.MinorPop()
			if err != nil {
				fmt.Printf("scale error: %s\n", err)
			}
			blas.AxpyFloat(ws3, z, 1.0)
			blas.ScalFloat(z, -1.0)

			checkpnt.Check("f3-call", minor+20)
			checkpnt.MinorPush(minor + 20)
			err = f3(x, y, z)
			checkpnt.MinorPop()
			checkpnt.Check("f3-return", minor+40)

			// Combine with solution of
			//
			// [ 0   A'  G'    ] [ x1         ]          [ c ]
			// [-A   0   0     ] [ y1         ] = -dgi * [ b ]
			// [-G   0   W'*W  ] [ W^{-1}*dzl ]          [ h ]
			//
			// to satisfy
			//
			// -c'*x - b'*y - h'*W^{-1}*z + dg*tau = btau - bkappa/tau. '

			// , kappa[0] := -kappa[0] / lmbd[-1] = -bkappa / lmbdag
			kap_ := kappa.Float()
			tau_ := tau.Float()
			kap_ = -kap_ / lmbda.GetIndex(-1)
			// tau[0] = tau[0] + kappa[0] / dgi = btau[0] - bkappa / tau
			tau_ = tau_ + kap_/dgi

			//tau[0] = dgi * ( tau[0] + xdot(c,x) + ydot(b,y) +
			//    misc.sdot(th, z, dims) ) / (1.0 + misc.sdot(z1, z1, dims))
			//tau_ = tau_ + blas.DotFloat(c, x) + blas.DotFloat(b, y) + sdot(th, z, dims, 0)
			tau_ += c.Dot(x)
			tau_ += b.Dot(y)
			tau_ += sdot(th, z, dims, 0)
			tau_ = dgi * tau_ / (1.0 + sdot(z1, z1, dims, 0))
			tau.SetValue(tau_)
			x1.Axpy(x, tau_)
			y1.Axpy(y, tau_)
			blas.AxpyFloat(z1, z, tau_)

			blas.AxpyFloat(z, s, -1.0)
			kap_ = kap_ - tau_
			kappa.SetValue(kap_)
			return
		}

		// f6(x, y, z, tau, s, kappa) solves the same system as f6_no_ir,
		// but applies iterative refinement. Following variables part of f6-closure
		// and ~ 12 is the limit. We wrap them to a structure.

		if iter == 0 {
			if refinement > 0 || solopts.Debug {
				WS.wx = c.Copy()
				WS.wy = b.Copy()
				WS.wz = matrix.FloatZeros(cdim, 1)
				WS.ws = matrix.FloatZeros(cdim, 1)
				WS.wtau = matrix.FloatValue(0.0)
				WS.wkappa = matrix.FloatValue(0.0)
				checkpnt.AddVerifiable("wx", WS.wx)
				checkpnt.AddMatrixVar("wz", WS.wz)
				checkpnt.AddMatrixVar("ws", WS.ws)
			}
			if refinement > 0 {
				WS.wx2 = c.Copy()
				WS.wy2 = b.Copy()
				WS.wz2 = matrix.FloatZeros(cdim, 1)
				WS.ws2 = matrix.FloatZeros(cdim, 1)
				WS.wtau2 = matrix.FloatValue(0.0)
				WS.wkappa2 = matrix.FloatValue(0.0)
				checkpnt.AddVerifiable("wx2", WS.wx2)
				checkpnt.AddMatrixVar("wz2", WS.wz2)
				checkpnt.AddMatrixVar("ws2", WS.ws2)
			}
		}

		f6 := func(x, y MatrixVariable, z, tau, s, kappa *matrix.FloatMatrix) error {
			var err error = nil
			minor := checkpnt.MinorTop()
			checkpnt.Check("startf6", minor+100)
			if refinement > 0 || solopts.Debug {
				mCopy(x, WS.wx)
				mCopy(y, WS.wy)
				blas.Copy(z, WS.wz)
				blas.Copy(s, WS.ws)
				WS.wtau.SetValue(tau.Float())
				WS.wkappa.SetValue(kappa.Float())
			}
			checkpnt.Check("pref6_no_ir", minor+200)
			err = f6_no_ir(x, y, z, tau, s, kappa)
			checkpnt.Check("postf6_no_ir", minor+399)
			for i := 0; i < refinement; i++ {
				mCopy(WS.wx, WS.wx2)
				mCopy(WS.wy, WS.wy2)
				blas.Copy(WS.wz, WS.wz2)
				blas.Copy(WS.ws, WS.ws2)
				WS.wtau2.SetValue(WS.wtau.Float())
				WS.wkappa2.SetValue(WS.wkappa.Float())
				checkpnt.Check("res-call", minor+400)

				checkpnt.MinorPush(minor + 400)
				err = res(x, y, z, tau, s, kappa, WS.wx2, WS.wy2, WS.wz2, WS.wtau2, WS.ws2, WS.wkappa2, W, dg, lmbda)
				checkpnt.MinorPop()

				checkpnt.Check("refine_pref6_no_ir", minor+500)
				checkpnt.MinorPush(minor + 500)
				err = f6_no_ir(WS.wx2, WS.wy2, WS.wz2, WS.wtau2, WS.ws2, WS.wkappa2)
				checkpnt.MinorPop()

				checkpnt.Check("refine_postf6_no_ir", minor+600)
				WS.wx2.Axpy(x, 1.0)
				WS.wy2.Axpy(y, 1.0)
				blas.AxpyFloat(WS.wz2, z, 1.0)
				blas.AxpyFloat(WS.ws2, s, 1.0)
				tau.SetValue(tau.Float() + WS.wtau2.Float())
				kappa.SetValue(kappa.Float() + WS.wkappa2.Float())
			}
			if solopts.Debug {
				checkpnt.MinorPush(minor + 700)
				res(x, y, z, tau, s, kappa, WS.wx, WS.wy, WS.wz, WS.wtau, WS.ws, WS.wkappa, W, dg, lmbda)
				checkpnt.MinorPop()
				fmt.Printf("KKT residuals\n")
				fmt.Printf("    'x'    : %.6e\n", math.Sqrt(WS.wx.Dot(WS.wx)))
				fmt.Printf("    'y'    : %.6e\n", math.Sqrt(WS.wy.Dot(WS.wy)))
				fmt.Printf("    'z'    : %.6e\n", snrm2(WS.wz, dims, 0))
				fmt.Printf("    'tau'  : %.6e\n", math.Abs(WS.wtau.Float()))
				fmt.Printf("    's'    : %.6e\n", snrm2(WS.ws, dims, 0))
				fmt.Printf("    'kappa': %.6e\n", math.Abs(WS.wkappa.Float()))
			}
			return err
		}

		var nrm float64 = blas.Nrm2Float(lmbda)
		mu := math.Pow(nrm, 2.0) / (1.0 + float64(cdim_diag))
		sigma := 0.0
		var step, tt, tk float64

		for i := 0; i < 2; i++ {
			// Solve
			//
			// [ 0         ]   [  0   A'  G'  c ] [ dx        ]
			// [ 0         ]   [ -A   0   0   b ] [ dy        ]
			// [ W'*ds     ] - [ -G   0   0   h ] [ W^{-1}*dz ]
			// [ dg*dkappa ]   [ -c' -b' -h'  0 ] [ dtau/dg   ]
			//
			//               [ rx   ]
			//               [ ry   ]
			// = - (1-sigma) [ rz   ]
			//               [ rtau ]
			//
			// lmbda o (dz + ds) = -lmbda o lmbda + sigma*mu*e
			// lmbdag * (dtau + dkappa) = - kappa * tau + sigma*mu
			//
			// ds = -lmbdasq if i is 0
			//    = -lmbdasq - dsa o dza + sigma*mu*e if i is 1
			// dkappa = -lambdasq[-1] if i is 0
			//        = -lambdasq[-1] - dkappaa*dtaua + sigma*mu if i is 1.
			ind := dims.Sum("l", "q")
			ind2 := ind
			blas.Copy(lmbdasq, ds, &la.IOpt{"n", ind})
			blas.ScalFloat(ds, 0.0, &la.IOpt{"offset", ind})
			for _, m := range dims.At("s") {
				blas.Copy(lmbdasq, ds, &la.IOpt{"n", m}, &la.IOpt{"offsetx", ind2},
					&la.IOpt{"offsety", ind}, &la.IOpt{"incy", m + 1})
				ind += m * m
				ind2 += m
			}
			// dkappa[0] = lmbdasq[-1]
			dkappa.SetValue(lmbdasq.GetIndex(-1))

			if i == 1 {
				blas.AxpyFloat(ws3, ds, 1.0)
				ind = dims.Sum("l", "q")
				is := make([]int, 0)
				// indexes: [:dims['l']]
				if dims.At("l")[0] > 0 {
					is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
				}
				// ...[indq[:-1]]
				if len(indq) > 1 {
					is = append(is, indq[:len(indq)-1]...)
				}
				// ...[ind : ind+m*m : m+1] (diagonal)
				for _, m := range dims.At("s") {
					is = append(is, matrix.MakeIndexSet(ind, ind+m*m, m+1)...)
					ind += m * m
				}
				//ds.Add(-sigma*mu, is...)
				for _, k := range is {
					ds.SetIndex(k, ds.GetIndex(k)-sigma*mu)
				}

				dk_ := dkappa.Float()
				wk_ := wkappa3.Float()
				dkappa.SetValue(dk_ + wk_ - sigma*mu)
			}
			// (dx, dy, dz, dtau) = (1-sigma)*(rx, ry, rz, rt)
			mCopy(rx, dx)
			dx.Scal(1.0 - sigma)
			mCopy(ry, dy)
			dy.Scal(1.0 - sigma)
			blas.Copy(rz, dz)
			blas.ScalFloat(dz, 1.0-sigma)
			// dtau[0] = (1.0 - sigma) * rt
			dtau.SetValue((1.0 - sigma) * rt)

			checkpnt.Check("pref6", (1+i)*1000)
			checkpnt.MinorPush((1 + i) * 1000)
			err = f6(dx, dy, dz, dtau, ds, dkappa)
			checkpnt.MinorPop()
			checkpnt.Check("postf6", (1+i)*1000+800)

			// Save ds o dz and dkappa * dtau for Mehrotra correction
			if i == 0 {
				blas.Copy(ds, ws3)
				sprod(ws3, dz, dims, 0)
				wkappa3.SetValue(dtau.Float() * dkappa.Float())
			}

			// Maximum step to boundary.
			//
			// If i is 1, also compute eigenvalue decomposition of the 's'
			// blocks in ds, dz.  The eigenvectors Qs, Qz are stored in
			// dsk, dzk.  The eigenvalues are stored in sigs, sigz.
			var ts, tz float64

			checkpnt.MinorPush((1+i)*1000 + 900)
			scale2(lmbda, ds, dims, 0, false)
			scale2(lmbda, dz, dims, 0, false)
			checkpnt.MinorPop()
			checkpnt.Check("post-scale2", (1+i)*1000+990)
			if i == 0 {
				ts, _ = maxStep(ds, dims, 0, nil)
				tz, _ = maxStep(dz, dims, 0, nil)
			} else {
				ts, _ = maxStep(ds, dims, 0, sigs)
				tz, _ = maxStep(dz, dims, 0, sigz)
			}
			dt_ := dtau.Float()
			dk_ := dkappa.Float()
			tt = -dt_ / lmbda.GetIndex(-1)
			tk = -dk_ / lmbda.GetIndex(-1)
			t := maxvec([]float64{0.0, ts, tz, tt, tk})
			if t == 0.0 {
				step = 1.0
			} else {
				if i == 0 {
					step = math.Min(1.0, 1.0/t)
				} else {
					step = math.Min(1.0, STEP/t)
				}
			}
			if i == 0 {
				// sigma = (1 - step)^3
				sigma = (1.0 - step) * (1.0 - step) * (1.0 - step)
				//sigma = math.Pow((1.0 - step), EXPON)
			}
		}
		//fmt.Printf("** tau = %.17f, kappa = %.17f\n", tau.Float(), kappa.Float())
		//fmt.Printf("** step = %.17f, sigma = %.17f\n", step, sigma)

		checkpnt.Check("update-xy", 7000)
		// Update x, y
		dx.Axpy(x, step)
		dy.Axpy(y, step)

		// Replace 'l' and 'q' blocks of ds and dz with the updated
		// variables in the current scaling.
		// Replace 's' blocks of ds and dz with the factors Ls, Lz in a
		// factorization Ls*Ls', Lz*Lz' of the updated variables in the
		// current scaling.
		//
		// ds := e + step*ds for 'l' and 'q' blocks.
		// dz := e + step*dz for 'l' and 'q' blocks.
		blas.ScalFloat(ds, step, &la.IOpt{"n", dims.Sum("l", "q")})
		blas.ScalFloat(dz, step, &la.IOpt{"n", dims.Sum("l", "q")})

		is := make([]int, 0)
		is = append(is, matrix.MakeIndexSet(0, dims.At("l")[0], 1)...)
		is = append(is, indq[:len(indq)-1]...)
		for _, k := range is {
			ds.SetIndex(k, 1.0+ds.GetIndex(k))
			dz.SetIndex(k, 1.0+dz.GetIndex(k))
		}
		checkpnt.Check("update-dsdz", 7500)

		// ds := H(lambda)^{-1/2} * ds and dz := H(lambda)^{-1/2} * dz.
		//
		// This replaces the 'l' and 'q' components of ds and dz with the
		// updated variables in the current scaling.
		// The 's' components of ds and dz are replaced with
		//
		// diag(lmbda_k)^{1/2} * Qs * diag(lmbda_k)^{1/2}
		// diag(lmbda_k)^{1/2} * Qz * diag(lmbda_k)^{1/2}
		checkpnt.MinorPush(7500)
		scale2(lmbda, ds, dims, 0, true)
		scale2(lmbda, dz, dims, 0, true)
		checkpnt.MinorPop()

		// sigs := ( e + step*sigs ) ./ lambda for 's' blocks.
		// sigz := ( e + step*sigz ) ./ lambda for 's' blocks.
		blas.ScalFloat(sigs, step)
		blas.ScalFloat(sigz, step)
		sigs.Add(1.0)
		sigz.Add(1.0)
		sdimsum := dims.Sum("s")
		qdimsum := dims.Sum("l", "q")
		blas.TbsvFloat(lmbda, sigs, &la.IOpt{"n", sdimsum}, &la.IOpt{"k", 0},
			&la.IOpt{"lda", 1}, &la.IOpt{"offseta", qdimsum})
		blas.TbsvFloat(lmbda, sigz, &la.IOpt{"n", sdimsum}, &la.IOpt{"k", 0},
			&la.IOpt{"lda", 1}, &la.IOpt{"offseta", qdimsum})

		ind2 := qdimsum
		ind3 := 0
		sdims := dims.At("s")
		for k := 0; k < len(sdims); k++ {
			m := sdims[k]
			for i := 0; i < m; i++ {
				a := math.Sqrt(sigs.GetIndex(ind3 + i))
				blas.ScalFloat(ds, a, &la.IOpt{"offset", ind2 + m*i}, &la.IOpt{"n", m})
				a = math.Sqrt(sigz.GetIndex(ind3 + i))
				blas.ScalFloat(dz, a, &la.IOpt{"offset", ind2 + m*i}, &la.IOpt{"n", m})
			}
			ind2 += m * m
			ind3 += m
		}

		checkpnt.Check("pre-update-scaling", 7700)
		err = updateScaling(W, lmbda, ds, dz)
		checkpnt.Check("post-update-scaling", 7800)

		// For kappa, tau block:
		//
		//     dg := sqrt( (kappa + step*dkappa) / (tau + step*dtau) )
		//         = dg * sqrt( (1 - step*tk) / (1 - step*tt) )
		//
		//     lmbda[-1] := sqrt((tau + step*dtau) * (kappa + step*dkappa))
		//                = lmbda[-1] * sqrt(( 1 - step*tt) * (1 - step*tk))
		dg *= math.Sqrt(1.0-step*tk) / math.Sqrt(1.0-step*tt)
		dgi = 1.0 / dg
		a := math.Sqrt(1.0-step*tk) * math.Sqrt(1.0-step*tt)
		lmbda.SetIndex(-1, a*lmbda.GetIndex(-1))

		// Unscale s, z, tau, kappa (unscaled variables are used only to
		// compute feasibility residuals).
		ind := dims.Sum("l", "q")
		ind2 = ind
		blas.Copy(lmbda, s, &la.IOpt{"n", ind})
		for _, m := range dims.At("s") {
			blas.ScalFloat(s, 0.0, &la.IOpt{"offset", ind2})
			blas.Copy(lmbda, s, &la.IOpt{"offsetx", ind}, &la.IOpt{"offsety", ind2},
				&la.IOpt{"n", m}, &la.IOpt{"incy", m + 1})
			ind += m
			ind2 += m * m
		}
		scale(s, W, true, false)

		ind = dims.Sum("l", "q")
		ind2 = ind
		blas.Copy(lmbda, z, &la.IOpt{"n", ind})
		for _, m := range dims.At("s") {
			blas.ScalFloat(z, 0.0, &la.IOpt{"offset", ind2})
			blas.Copy(lmbda, z, &la.IOpt{"offsetx", ind}, &la.IOpt{"offsety", ind2},
				&la.IOpt{"n", m}, &la.IOpt{"incy", m + 1})
			ind += m
			ind2 += m * m
		}
		scale(z, W, false, true)

		kappa.SetValue(lmbda.GetIndex(-1) / dgi)
		tau.SetValue(lmbda.GetIndex(-1) * dgi)
		g := blas.Nrm2Float(lmbda, &la.IOpt{"n", lmbda.Rows() - 1}) / tau.Float()
		gap = g * g
		checkpnt.Check("end-of-loop", 8000)
		//fmt.Printf(" ** kappa=%.10f, tau=%.10f, gap=%.10f\n", kappa.Float(), tau.Float(), gap)

	}
	return
}
Exemple #4
0
func kktChol2(G *matrix.FloatMatrix, dims *sets.DimensionSet, A *matrix.FloatMatrix, mnl int) (kktFactor, error) {

	if len(dims.At("q")) > 0 || len(dims.At("s")) > 0 {
		return nil, errors.New("'chol2' solver only for problems with no second-order or " +
			"semidefinite cone constraints")
	}

	p, n := A.Size()
	ml := dims.At("l")[0]
	F := &chol2Data{firstcall: true, singular: false, A: A, G: G, dims: dims}

	factor := func(W *sets.FloatMatrixSet, H, Df *matrix.FloatMatrix) (KKTFunc, error) {
		var err error = nil
		minor := 0
		if !checkpnt.MinorEmpty() {
			minor = checkpnt.MinorTop()
		}
		if F.firstcall {
			F.Gs = matrix.FloatZeros(F.G.Size())
			if mnl > 0 {
				F.Dfs = matrix.FloatZeros(Df.Size())
			}
			F.S = matrix.FloatZeros(n, n)
			F.K = matrix.FloatZeros(p, p)
			checkpnt.AddMatrixVar("Gs", F.Gs)
			checkpnt.AddMatrixVar("Dfs", F.Dfs)
			checkpnt.AddMatrixVar("S", F.S)
			checkpnt.AddMatrixVar("K", F.K)
		}

		if mnl > 0 {
			dnli := matrix.FloatZeros(mnl, mnl)
			dnli.SetIndexesFromArray(W.At("dnli")[0].FloatArray(), matrix.DiagonalIndexes(dnli)...)
			blas.GemmFloat(dnli, Df, F.Dfs, 1.0, 0.0)
		}
		checkpnt.Check("02factor_chol2", minor)
		di := matrix.FloatZeros(ml, ml)
		di.SetIndexesFromArray(W.At("di")[0].FloatArray(), matrix.DiagonalIndexes(di)...)
		err = blas.GemmFloat(di, G, F.Gs, 1.0, 0.0)
		checkpnt.Check("06factor_chol2", minor)

		if F.firstcall {
			blas.SyrkFloat(F.Gs, F.S, 1.0, 0.0, la.OptTrans)
			if mnl > 0 {
				blas.SyrkFloat(F.Dfs, F.S, 1.0, 1.0, la.OptTrans)
			}
			if H != nil {
				F.S.Plus(H)
			}
			checkpnt.Check("10factor_chol2", minor)
			err = lapack.Potrf(F.S)
			if err != nil {
				err = nil // reset error
				F.singular = true
				// original code recreates F.S as dense if it is sparse and
				// A is dense, we don't do it as currently no sparse matrices
				//F.S = matrix.FloatZeros(n, n)
				//checkpnt.AddMatrixVar("S", F.S)
				blas.SyrkFloat(F.Gs, F.S, 1.0, 0.0, la.OptTrans)
				if mnl > 0 {
					blas.SyrkFloat(F.Dfs, F.S, 1.0, 1.0, la.OptTrans)
				}
				checkpnt.Check("14factor_chol2", minor)
				blas.SyrkFloat(F.A, F.S, 1.0, 1.0, la.OptTrans)
				if H != nil {
					F.S.Plus(H)
				}
				lapack.Potrf(F.S)
			}
			F.firstcall = false
			checkpnt.Check("20factor_chol2", minor)
		} else {
			blas.SyrkFloat(F.Gs, F.S, 1.0, 0.0, la.OptTrans)
			if mnl > 0 {
				blas.SyrkFloat(F.Dfs, F.S, 1.0, 1.0, la.OptTrans)
			}
			if H != nil {
				F.S.Plus(H)
			}
			checkpnt.Check("40factor_chol2", minor)
			if F.singular {
				blas.SyrkFloat(F.A, F.S, 1.0, 1.0, la.OptTrans)
			}
			lapack.Potrf(F.S)
			checkpnt.Check("50factor_chol2", minor)
		}

		// Asct := L^{-1}*A'.  Factor K = Asct'*Asct.
		Asct := F.A.Transpose()
		blas.TrsmFloat(F.S, Asct, 1.0)
		blas.SyrkFloat(Asct, F.K, 1.0, 0.0, la.OptTrans)
		lapack.Potrf(F.K)
		checkpnt.Check("90factor_chol2", minor)

		solve := func(x, y, z *matrix.FloatMatrix) (err error) {
			// Solve
			//
			//     [ H          A'  GG'*W^{-1} ]   [ ux   ]   [ bx        ]
			//     [ A          0   0          ] * [ uy   ] = [ by        ]
			//     [ W^{-T}*GG  0   -I         ]   [ W*uz ]   [ W^{-T}*bz ]
			//
			// and return ux, uy, W*uz.
			//
			// If not F['singular']:
			//
			//     K*uy = A * S^{-1} * ( bx + GG'*W^{-1}*W^{-T}*bz ) - by
			//     S*ux = bx + GG'*W^{-1}*W^{-T}*bz - A'*uy
			//     W*uz = W^{-T} * ( GG*ux - bz ).
			//
			// If F['singular']:
			//
			//     K*uy = A * S^{-1} * ( bx + GG'*W^{-1}*W^{-T}*bz + A'*by )
			//            - by
			//     S*ux = bx + GG'*W^{-1}*W^{-T}*bz + A'*by - A'*y.
			//     W*uz = W^{-T} * ( GG*ux - bz ).

			minor := 0
			if !checkpnt.MinorEmpty() {
				minor = checkpnt.MinorTop()
			}

			// z := W^{-1} * z = W^{-1} * bz
			scale(z, W, true, true)
			checkpnt.Check("10solve_chol2", minor)

			// If not F['singular']:
			//     x := L^{-1} * P * (x + GGs'*z)
			//        = L^{-1} * P * (x + GG'*W^{-1}*W^{-T}*bz)
			//
			// If F['singular']:
			//     x := L^{-1} * P * (x + GGs'*z + A'*y))
			//        = L^{-1} * P * (x + GG'*W^{-1}*W^{-T}*bz + A'*y)
			if mnl > 0 {
				blas.GemvFloat(F.Dfs, z, x, 1.0, 1.0, la.OptTrans)
			}
			blas.GemvFloat(F.Gs, z, x, 1.0, 1.0, la.OptTrans, &la.IOpt{"offsetx", mnl})
			//checkpnt.Check("20solve_chol2", minor)
			if F.singular {
				blas.GemvFloat(F.A, y, x, 1.0, 1.0, la.OptTrans)
			}
			checkpnt.Check("30solve_chol2", minor)
			blas.TrsvFloat(F.S, x)
			//checkpnt.Check("50solve_chol2", minor)

			// y := K^{-1} * (Asc*x - y)
			//    = K^{-1} * (A * S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz) - by)
			//      (if not F['singular'])
			//    = K^{-1} * (A * S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz +
			//      A'*by) - by)
			//      (if F['singular']).
			blas.GemvFloat(Asct, x, y, 1.0, -1.0, la.OptTrans)
			//checkpnt.Check("55solve_chol2", minor)
			lapack.Potrs(F.K, y)
			//checkpnt.Check("60solve_chol2", minor)

			// x := P' * L^{-T} * (x - Asc'*y)
			//    = S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz - A'*y)
			//      (if not F['singular'])
			//    = S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz + A'*by - A'*y)
			//      (if F['singular'])
			blas.GemvFloat(Asct, y, x, -1.0, 1.0)
			blas.TrsvFloat(F.S, x, la.OptTrans)
			checkpnt.Check("70solve_chol2", minor)

			// W*z := GGs*x - z = W^{-T} * (GG*x - bz)
			if mnl > 0 {
				blas.GemvFloat(F.Dfs, x, z, 1.0, -1.0)
			}
			blas.GemvFloat(F.Gs, x, z, 1.0, -1.0, &la.IOpt{"offsety", mnl})

			checkpnt.Check("90solve_chol2", minor)
			return nil
		}
		return solve, err
	}
	return factor, nil
}
Exemple #5
0
//    Solution of KKT equations by reduction to a 2 x 2 system, a QR
//    factorization to eliminate the equality constraints, and a dense
//    Cholesky factorization of order n-p.
//
//    Computes the QR factorization
//
//        A' = [Q1, Q2] * [R; 0]
//
//    and returns a function that (1) computes the Cholesky factorization
//
//        Q_2^T * (H + GG^T * W^{-1} * W^{-T} * GG) * Q2 = L * L^T,
//
//    given H, Df, W, where GG = [Df; G], and (2) returns a function for
//    solving
//
//        [ H    A'   GG'    ]   [ ux ]   [ bx ]
//        [ A    0    0      ] * [ uy ] = [ by ].
//        [ GG   0    -W'*W  ]   [ uz ]   [ bz ]
//
//    H is n x n,  A is p x n, Df is mnl x n, G is N x n where
//    N = dims['l'] + sum(dims['q']) + sum( k**2 for k in dims['s'] ).
//
func kktChol(G *matrix.FloatMatrix, dims *sets.DimensionSet, A *matrix.FloatMatrix, mnl int) (kktFactor, error) {

	p, n := A.Size()
	cdim := mnl + dims.Sum("l", "q") + dims.SumSquared("s")
	cdim_pckd := mnl + dims.Sum("l", "q") + dims.SumPacked("s")

	QA := A.Transpose()
	tauA := matrix.FloatZeros(p, 1)
	lapack.Geqrf(QA, tauA)

	Gs := matrix.FloatZeros(cdim, n)
	K := matrix.FloatZeros(n, n)
	bzp := matrix.FloatZeros(cdim_pckd, 1)
	yy := matrix.FloatZeros(p, 1)
	checkpnt.AddMatrixVar("tauA", tauA)
	checkpnt.AddMatrixVar("Gs", Gs)
	checkpnt.AddMatrixVar("K", K)

	factor := func(W *sets.FloatMatrixSet, H, Df *matrix.FloatMatrix) (KKTFunc, error) {
		// Compute
		//
		//     K = [Q1, Q2]' * (H + GG' * W^{-1} * W^{-T} * GG) * [Q1, Q2]
		//
		// and take the Cholesky factorization of the 2,2 block
		//
		//     Q_2' * (H + GG^T * W^{-1} * W^{-T} * GG) * Q2.

		var err error = nil
		minor := 0
		if !checkpnt.MinorEmpty() {
			minor = checkpnt.MinorTop()
		}
		// Gs = W^{-T} * GG in packed storage.
		if mnl > 0 {
			Gs.SetSubMatrix(0, 0, Df)
		}
		Gs.SetSubMatrix(mnl, 0, G)
		checkpnt.Check("00factor_chol", minor)
		scale(Gs, W, true, true)
		pack2(Gs, dims, mnl)
		//checkpnt.Check("10factor_chol", minor)

		// K = [Q1, Q2]' * (H + Gs' * Gs) * [Q1, Q2].
		blas.SyrkFloat(Gs, K, 1.0, 0.0, la.OptTrans, &la.IOpt{"k", cdim_pckd})
		if H != nil {
			K.SetSubMatrix(0, 0, matrix.Plus(H, K.GetSubMatrix(0, 0, H.Rows(), H.Cols())))
		}
		//checkpnt.Check("20factor_chol", minor)
		symm(K, n, 0)
		lapack.Ormqr(QA, tauA, K, la.OptLeft, la.OptTrans)
		lapack.Ormqr(QA, tauA, K, la.OptRight)
		//checkpnt.Check("30factor_chol", minor)

		// Cholesky factorization of 2,2 block of K.
		lapack.Potrf(K, &la.IOpt{"n", n - p}, &la.IOpt{"offseta", p * (n + 1)})
		checkpnt.Check("40factor_chol", minor)

		solve := func(x, y, z *matrix.FloatMatrix) (err error) {
			// Solve
			//
			//     [ 0          A'  GG'*W^{-1} ]   [ ux   ]   [ bx        ]
			//     [ A          0   0          ] * [ uy   ] = [ by        ]
			//     [ W^{-T}*GG  0   -I         ]   [ W*uz ]   [ W^{-T}*bz ]
			//
			// and return ux, uy, W*uz.
			//
			// On entry, x, y, z contain bx, by, bz.  On exit, they contain
			// the solution ux, uy, W*uz.
			//
			// If we change variables ux = Q1*v + Q2*w, the system becomes
			//
			//     [ K11 K12 R ]   [ v  ]   [Q1'*(bx+GG'*W^{-1}*W^{-T}*bz)]
			//     [ K21 K22 0 ] * [ w  ] = [Q2'*(bx+GG'*W^{-1}*W^{-T}*bz)]
			//     [ R^T 0   0 ]   [ uy ]   [by                           ]
			//
			//     W*uz = W^{-T} * ( GG*ux - bz ).
			minor := 0
			if !checkpnt.MinorEmpty() {
				minor = checkpnt.MinorTop()
			}

			// bzp := W^{-T} * bz in packed storage
			scale(z, W, true, true)
			pack(z, bzp, dims, &la.IOpt{"mnl", mnl})

			// x := [Q1, Q2]' * (x + Gs' * bzp)
			//    = [Q1, Q2]' * (bx + Gs' * W^{-T} * bz)
			blas.GemvFloat(Gs, bzp, x, 1.0, 1.0, la.OptTrans, &la.IOpt{"m", cdim_pckd})
			lapack.Ormqr(QA, tauA, x, la.OptLeft, la.OptTrans)

			// y := x[:p]
			//    = Q1' * (bx + Gs' * W^{-T} * bz)
			blas.Copy(y, yy)
			blas.Copy(x, y, &la.IOpt{"n", p})

			// x[:p] := v = R^{-T} * by
			blas.Copy(yy, x)
			lapack.Trtrs(QA, x, la.OptUpper, la.OptTrans, &la.IOpt{"n", p})

			// x[p:] := K22^{-1} * (x[p:] - K21*x[:p])
			//        = K22^{-1} * (Q2' * (bx + Gs' * W^{-T} * bz) - K21*v)
			blas.GemvFloat(K, x, x, -1.0, 1.0, &la.IOpt{"m", n - p}, &la.IOpt{"n", p},
				&la.IOpt{"offseta", p}, &la.IOpt{"offsety", p})
			lapack.Potrs(K, x, &la.IOpt{"n", n - p}, &la.IOpt{"offseta", p * (n + 1)},
				&la.IOpt{"offsetb", p})

			// y := y - [K11, K12] * x
			//    = Q1' * (bx + Gs' * W^{-T} * bz) - K11*v - K12*w
			blas.GemvFloat(K, x, y, -1.0, 1.0, &la.IOpt{"m", p}, &la.IOpt{"n", n})

			// y := R^{-1}*y
			//    = R^{-1} * (Q1' * (bx + Gs' * W^{-T} * bz) - K11*v
			//      - K12*w)
			lapack.Trtrs(QA, y, la.OptUpper, &la.IOpt{"n", p})

			// x := [Q1, Q2] * x
			lapack.Ormqr(QA, tauA, x, la.OptLeft)

			// bzp := Gs * x - bzp.
			//      = W^{-T} * ( GG*ux - bz ) in packed storage.
			// Unpack and copy to z.
			blas.GemvFloat(Gs, x, bzp, 1.0, -1.0, &la.IOpt{"m", cdim_pckd})
			unpack(bzp, z, dims, &la.IOpt{"mnl", mnl})

			checkpnt.Check("90solve_chol", minor)
			return nil
		}
		return solve, err
	}
	return factor, nil
}
Exemple #6
0
// Solution of KKT equations with zero 1,1 block, by eliminating the
// equality constraints via a QR factorization, and solving the
// reduced KKT system by another QR factorization.
//
// Computes the QR factorization
//
//        A' = [Q1, Q2] * [R1; 0]
//
// and returns a function that (1) computes the QR factorization
//
//        W^{-T} * G * Q2 = Q3 * R3
//
// (with columns of W^{-T}*G in packed storage), and (2) returns a function for solving
//
//        [ 0    A'   G'    ]   [ ux ]   [ bx ]
//        [ A    0    0     ] * [ uy ] = [ by ].
//        [ G    0   -W'*W  ]   [ uz ]   [ bz ]
//
// A is p x n and G is N x n where N = dims['l'] + sum(dims['q']) +
// sum( k**2 for k in dims['s'] ).
//
func kktQr(G *matrix.FloatMatrix, dims *sets.DimensionSet, A *matrix.FloatMatrix, mnl int) (kktFactor, error) {

	p, n := A.Size()
	cdim := dims.Sum("l", "q") + dims.SumSquared("s")
	cdim_pckd := dims.Sum("l", "q") + dims.SumPacked("s")

	QA := A.Transpose()
	tauA := matrix.FloatZeros(p, 1)
	lapack.Geqrf(QA, tauA)

	Gs := matrix.FloatZeros(cdim, n)
	tauG := matrix.FloatZeros(n-p, 1)
	u := matrix.FloatZeros(cdim_pckd, 1)
	vv := matrix.FloatZeros(n, 1)
	w := matrix.FloatZeros(cdim_pckd, 1)
	checkpnt.AddMatrixVar("tauA", tauA)
	checkpnt.AddMatrixVar("tauG", tauG)
	checkpnt.AddMatrixVar("Gs", Gs)
	checkpnt.AddMatrixVar("qr_u", u)
	checkpnt.AddMatrixVar("qr_vv", vv)

	factor := func(W *sets.FloatMatrixSet, H, Df *matrix.FloatMatrix) (KKTFunc, error) {
		var err error = nil
		minor := 0
		if !checkpnt.MinorEmpty() {
			minor = checkpnt.MinorTop()
		}

		// Gs = W^{-T}*G, in packed storage.
		blas.Copy(G, Gs)
		//checkpnt.Check("00factor_qr", minor)
		scale(Gs, W, true, true)
		//checkpnt.Check("01factor_qr", minor)
		pack2(Gs, dims, 0)
		//checkpnt.Check("02factor_qr", minor)

		// Gs := [ Gs1, Gs2 ]
		//     = Gs * [ Q1, Q2 ]
		lapack.Ormqr(QA, tauA, Gs, la.OptRight, &la.IOpt{"m", cdim_pckd})
		//checkpnt.Check("03factor_qr", minor)

		// QR factorization Gs2 := [ Q3, Q4 ] * [ R3; 0 ]
		lapack.Geqrf(Gs, tauG, &la.IOpt{"n", n - p}, &la.IOpt{"m", cdim_pckd},
			&la.IOpt{"offseta", Gs.Rows() * p})
		checkpnt.Check("10factor_qr", minor)

		solve := func(x, y, z *matrix.FloatMatrix) (err error) {
			// On entry, x, y, z contain bx, by, bz.  On exit, they
			// contain the solution x, y, W*z of
			//
			//     [ 0         A'  G'*W^{-1} ]   [ x   ]   [bx       ]
			//     [ A         0   0         ] * [ y   ] = [by       ].
			//     [ W^{-T}*G  0   -I        ]   [ W*z ]   [W^{-T}*bz]
			//
			// The system is solved in five steps:
			//
			//       w := W^{-T}*bz - Gs1*R1^{-T}*by
			//       u := R3^{-T}*Q2'*bx + Q3'*w
			//     W*z := Q3*u - w
			//       y := R1^{-1} * (Q1'*bx - Gs1'*(W*z))
			//       x := [ Q1, Q2 ] * [ R1^{-T}*by;  R3^{-1}*u ]

			minor := 0
			if !checkpnt.MinorEmpty() {
				minor = checkpnt.MinorTop()
			}

			// w := W^{-T} * bz in packed storage
			scale(z, W, true, true)
			pack(z, w, dims)
			//checkpnt.Check("00solve_qr", minor)

			// vv := [ Q1'*bx;  R3^{-T}*Q2'*bx ]
			blas.Copy(x, vv)
			lapack.Ormqr(QA, tauA, vv, la.OptTrans)
			lapack.Trtrs(Gs, vv, la.OptUpper, la.OptTrans, &la.IOpt{"n", n - p},
				&la.IOpt{"offseta", Gs.Rows() * p}, &la.IOpt{"offsetb", p})
			//checkpnt.Check("10solve_qr", minor)

			// x[:p] := R1^{-T} * by
			blas.Copy(y, x)
			lapack.Trtrs(QA, x, la.OptUpper, la.OptTrans, &la.IOpt{"n", p})
			//checkpnt.Check("20solve_qr", minor)

			// w := w - Gs1 * x[:p]
			//    = W^{-T}*bz - Gs1*by
			blas.GemvFloat(Gs, x, w, -1.0, 1.0, &la.IOpt{"n", p}, &la.IOpt{"m", cdim_pckd})
			//checkpnt.Check("30solve_qr", minor)

			// u := [ Q3'*w + v[p:];  0 ]
			//    = [ Q3'*w + R3^{-T}*Q2'*bx; 0 ]
			blas.Copy(w, u)
			lapack.Ormqr(Gs, tauG, u, la.OptTrans, &la.IOpt{"k", n - p},
				&la.IOpt{"offseta", Gs.Rows() * p}, &la.IOpt{"m", cdim_pckd})
			blas.AxpyFloat(vv, u, 1.0, &la.IOpt{"offsetx", p}, &la.IOpt{"n", n - p})
			blas.ScalFloat(u, 0.0, &la.IOpt{"offset", n - p})
			//checkpnt.Check("40solve_qr", minor)

			// x[p:] := R3^{-1} * u[:n-p]
			blas.Copy(u, x, &la.IOpt{"offsety", p}, &la.IOpt{"n", n - p})
			lapack.Trtrs(Gs, x, la.OptUpper, &la.IOpt{"n", n - p},
				&la.IOpt{"offset", Gs.Rows() * p}, &la.IOpt{"offsetb", p})
			//checkpnt.Check("50solve_qr", minor)

			// x is now [ R1^{-T}*by;  R3^{-1}*u[:n-p] ]
			// x := [Q1 Q2]*x
			lapack.Ormqr(QA, tauA, x)
			//checkpnt.Check("60solve_qr", minor)

			// u := [Q3, Q4] * u - w
			//    = Q3 * u[:n-p] - w
			lapack.Ormqr(Gs, tauG, u, &la.IOpt{"k", n - p}, &la.IOpt{"m", cdim_pckd},
				&la.IOpt{"offseta", Gs.Rows() * p})
			blas.AxpyFloat(w, u, -1.0)
			//checkpnt.Check("70solve_qr", minor)

			// y := R1^{-1} * ( v[:p] - Gs1'*u )
			//    = R1^{-1} * ( Q1'*bx - Gs1'*u )
			blas.Copy(vv, y, &la.IOpt{"n", p})
			blas.GemvFloat(Gs, u, y, -1.0, 1.0, &la.IOpt{"m", cdim_pckd},
				&la.IOpt{"n", p}, la.OptTrans)
			lapack.Trtrs(QA, y, la.OptUpper, &la.IOpt{"n", p})
			//checkpnt.Check("80solve_qr", minor)

			unpack(u, z, dims)
			checkpnt.Check("90solve_qr", minor)
			return nil
		}
		return solve, err
	}
	return factor, nil
}