library(OpenMx)
mxVersion() 
mxOption(NULL,"Default optimizer","NPSOL")
data(twinData)
twinData <- twinData[twinData$zyg %in% 1:5, ]
twinData$zyg2 <- sapply(twinData$zyg, switch, 1, 1, 2, 2, 2)

def_sat_model <- function(data, zyg, vars) {
	# zyg - zygosity variable (character string)
	# vars - phenotypic variables (character string or vector)
	nv <- length(vars)
	selvars <- paste0(vars, rep(1:2, each = nv))
	
	# Split MZ and DZ data
	mzdata <- data[data[, zyg] == 1, selvars]
	dzdata <- data[data[, zyg] == 2, selvars]
	
	# Starting values - These are for this particular data
	startChol <- matrix(c(0.8, 2.7, 0.4, 1.3,
												0.0, 0.3, 0.0, 0.0,
												0.0, 0.0, 0.7, 2.4,
												0.0, 0.0, 0.0, 0.3),
											ncol = 4, byrow = TRUE)
	
	# Selector matrix for the omxSelectRowsAndCols
	#selecttw1 <- matrix(c(rep(1, nv), rep(0, nv)), ncol = 1)
	#selecttw2 <- matrix(c(rep(0, nv), rep(1, nv)), ncol = 1)
	
	mxModel(
		name = 'Saturated',
		mxMatrix(type = 'Lower', nrow = 2 * nv, ncol = 2 * nv,
						 free = TRUE, values = t(startChol), name = 'CholMZ'),
		mxMatrix(type = 'Lower', nrow = 2 * nv, ncol = 2 * nv,
						 free = TRUE, values = t(startChol), name = 'CholDZ'),
		mxAlgebra(CholMZ %*% t(CholMZ), name = 'expCovMZ'),
		mxAlgebra(CholDZ %*% t(CholDZ), name = 'expCovDZ'),
		mxMatrix(type="Full",nrow=4,ncol=1,free=F,values=c(rep(1, nv), rep(0, nv)),name="selecttw1"),
		mxMatrix(type="Full",nrow=4,ncol=1,free=F,values=c(rep(0, nv), rep(1, nv)),name="selecttw2"),
		
		mxConstraint(omxSelectRowsAndCols(expCovMZ, selecttw1) ==
								 	omxSelectRowsAndCols(expCovMZ, selecttw2)),
		mxConstraint(omxSelectRowsAndCols(expCovDZ, selecttw1) ==
								 	omxSelectRowsAndCols(expCovDZ, selecttw2)),
		mxConstraint(omxSelectRowsAndCols(expCovDZ, selecttw1) ==
								 	omxSelectRowsAndCols(expCovMZ, selecttw1)),
		
		mxMatrix(type = 'Full', nrow = 1, ncol = nv,
						 free = TRUE, values = c(21, 21), name = 'Mean'),
		mxAlgebra(cbind(Mean, Mean), name = 'expMeans'),
		
		mxModel(name = 'MZ',
						mxData(type = 'raw', observed = mzdata),
						mxExpectationNormal('Saturated.expCovMZ', 'Saturated.expMeans',
																dimnames = selvars),
						mxFitFunctionML()),
		mxModel(name = 'DZ',
						mxData(type = 'raw', observed = dzdata),
						mxExpectationNormal('Saturated.expCovDZ', 'Saturated.expMeans',
																dimnames = selvars),
						mxFitFunctionML()),
		mxFitFunctionMultigroup(c('MZ', 'DZ'))
	)
}
vars <- c('bmi', 'htwt')
sat_model <- def_sat_model(twinData, 'zyg2', vars)

set.seed(9875382)
#NPSOL can reach the solution with mxTryHard():
fitN <- mxTryHard(sat_model)
summary(fitN) 
fitN$output$constraintFunctionValues
dim(fitN$output$constraintJacobian)
qr(fitN$output$constraintJacobian)$rank #<--Jacobian is rank-deficient.

mxOption(NULL,"Default optimizer","SLSQP")
set.seed(9875382)
fitS <- mxTryHard(sat_model)
summary(fitS) #<--Status code 3
fitS$output$constraintFunctionValues #<--Constraints not satisfied within feasibility tolerance.
dim(fitS$output$constraintJacobian)
qr(fitS$output$constraintJacobian)$rank

# CSOLNP spins its wheels (but at least it's interruptible now):
# mxOption(NULL,"Default optimizer","CSOLNP")
# fitC <- mxRun(sat_model)


def_sat_model2 <- function(data, zyg, vars) {
	# zyg - zygosity variable (character string)
	# vars - phenotypic variables (character string or vector)
	nv <- length(vars)
	selvars <- paste0(vars, rep(1:2, each = nv))
	
	# Split MZ and DZ data
	mzdata <- data[data[, zyg] == 1, selvars]
	dzdata <- data[data[, zyg] == 2, selvars]
	
	# Starting values - These are for this particular data
	startChol <- matrix(c(0.8, 2.7, 0.4, 1.3,
												0.0, 0.3, 0.0, 0.0,
												0.0, 0.0, 0.7, 2.4,
												0.0, 0.0, 0.0, 0.3),
											ncol = 4, byrow = TRUE)
	
	# Selector matrix for the omxSelectRowsAndCols
	#selecttw1 <- matrix(c(rep(1, nv), rep(0, nv)), ncol = 1)
	#selecttw2 <- matrix(c(rep(0, nv), rep(1, nv)), ncol = 1)
	
	mxModel(
		name = 'Saturated',
		mxMatrix(type = 'Lower', nrow = 2 * nv, ncol = 2 * nv,
						 free = TRUE, values = t(startChol), name = 'CholMZ'),
		mxMatrix(type = 'Lower', nrow = 2 * nv, ncol = 2 * nv,
						 free = TRUE, values = t(startChol), name = 'CholDZ'),
		mxAlgebra(CholMZ %*% t(CholMZ), name = 'expCovMZ'),
		mxAlgebra(CholDZ %*% t(CholDZ), name = 'expCovDZ'),
		mxMatrix(type="Full",nrow=4,ncol=1,free=F,values=c(rep(1, nv), rep(0, nv)),name="selecttw1"),
		mxMatrix(type="Full",nrow=4,ncol=1,free=F,values=c(rep(0, nv), rep(1, nv)),name="selecttw2"),
		
		# mxConstraint(omxSelectRowsAndCols(expCovMZ, selecttw1) ==
		# 						 	omxSelectRowsAndCols(expCovMZ, selecttw2)),
		# mxConstraint(omxSelectRowsAndCols(expCovDZ, selecttw1) ==
		# 						 	omxSelectRowsAndCols(expCovDZ, selecttw2)),
		# mxConstraint(omxSelectRowsAndCols(expCovDZ, selecttw1) ==
		# 						 	omxSelectRowsAndCols(expCovMZ, selecttw1)),
		mxConstraint(expCovMZ[1:2,1]==expCovMZ[3:4,3]),
		mxConstraint(expCovMZ[2,2]==expCovMZ[4,4]),
		mxConstraint(expCovDZ[1:2,1]==expCovDZ[3:4,3]),
		mxConstraint(expCovDZ[2,2]==expCovDZ[4,4]),
		mxConstraint(expCovDZ[1:2,1]==expCovMZ[1:2,1]),
		mxConstraint(expCovDZ[2,2]==expCovMZ[2,2]),
		
		mxMatrix(type = 'Full', nrow = 1, ncol = nv,
						 free = TRUE, values = c(21, 21), name = 'Mean'),
		mxAlgebra(cbind(Mean, Mean), name = 'expMeans'),
		
		mxModel(name = 'MZ',
						mxData(type = 'raw', observed = mzdata),
						mxExpectationNormal('Saturated.expCovMZ', 'Saturated.expMeans',
																dimnames = selvars),
						mxFitFunctionML()),
		mxModel(name = 'DZ',
						mxData(type = 'raw', observed = dzdata),
						mxExpectationNormal('Saturated.expCovDZ', 'Saturated.expMeans',
																dimnames = selvars),
						mxFitFunctionML()),
		mxFitFunctionMultigroup(c('MZ', 'DZ'))
	)
}

sat_model2 <- def_sat_model2(twinData, 'zyg2', vars)

mxOption(NULL,"Default optimizer","NPSOL")
set.seed(9875382)
#For some reason, NPSOL doesn't reach the solution now that there are no redundant equality constraints:
fit2N <- mxTryHard(sat_model2)
summary(fit2N) #<--Poor solution, but is at least feasible
fit2N$output$constraintFunctionValues
dim(fit2N$output$constraintJacobian)
qr(fit2N$output$constraintJacobian)$rank #<--Jacobian is full rank.

mxOption(NULL,"Default optimizer","SLSQP")
set.seed(9875382)
fit2S <- mxTryHard(sat_model2)
summary(fit2S) #<--Without the redundant constraints, SLSQP can reach the solution with mxTryHard.
dim(fit2S$output$constraintJacobian)
qr(fit2S$output$constraintJacobian)$rank
fit2S$output$constraintFunctionValues

mxOption(NULL,"Default optimizer","CSOLNP")
#CSOLNP can get the solution on the first try:
fit2C <- mxRun(sat_model2)
summary(fit2C)
dim(fit2C$output$constraintJacobian)
qr(fit2C$output$constraintJacobian)$rank
fit2C$output$constraintFunctionValues

# NPSOL and SLSQP can reach the solution on one try with an assist from Nelder-Mead: ###

mxOption(NULL,"Default optimizer","NPSOL")
plan <- omxDefaultComputePlan()
plan$steps <- list(
	NM=mxComputeNelderMead(iniSimplexType="regular"),GD=plan$steps$GD,ND=plan$steps$ND,SE=plan$steps$SE,RD=plan$steps$RD,RE=plan$steps$RD)
hybridN <- mxModel(sat_model2,plan)
hybridN <- mxOption(hybridN,"Feasibility tolerance",0.001)
hybridfitN <- mxRun(hybridN)
summary(hybridfitN)

mxOption(NULL,"Default optimizer","SLSQP")
plan <- omxDefaultComputePlan()
plan$steps <- list(
	NM=mxComputeNelderMead(iniSimplexType="regular"),GD=plan$steps$GD,ND=plan$steps$ND,SE=plan$steps$SE,RD=plan$steps$RD,RE=plan$steps$RD)
hybridS <- mxModel(sat_model2,plan)
hybridS <- mxOption(hybridS,"Feasibility tolerance",0.001)
hybridfitS <- mxRun(hybridS)
summary(hybridfitS)