#
# these are functions for multidimensional scaling. they handle weights, 
# metric and non-metric, constraints on the configaration,
# and various types of individual differences models. 
#
# version 0.5.01 2005-10-08
# version 0.6.00 2006-02-22 (unified two versions)
# version 0.7.00 2007-05-11 (added spherical constraints)
#

require("pava")
require("mdsutils")

#
# Chapter1: Basic SMACOF for Sysmmetric and Rectangular Matrices
#

smacofSym<-function(
	diss,
	wgths=initWeights(diss),
	p=2,
	init="none",
	metric=TRUE,
	ties="primary",
	verbose=FALSE,
	relax=1,
	modulus=1,
	itmax=100,
	eps=1e-6) {
n<-attr(diss,"Size"); m<-length(diss); dhat<-normDiss(diss,wgths)
if (!is.matrix(init)) x<-torgerson(sqrt(delta),p=p) else x<-init
if (!is.matrix(w)) w<-matrix(1,n,n)-diag(n)
w<-vmat(wgths); v<-myGenInv(w); itel<-1;
d<-dist(x); lb<-sum(wgths*d*dhat)/sum(wgths*d^2); x<-lb*x; d<-lb*d
sold<-sum(wgths*(dhat-d)^2)
repeat {
	b<-bmat(dhat,wgths,d); y<-v%*%b%*%x
	y<-x+relax*(y-x); e<-dist(y)
	ssma<-sum(wgths*(dhat-e)^2)
	if (!metric) {
	    if ((itel%%modulus) == 0) {
			if (ties=="primary") daux<-monregP(diss,e,wgths)
			if (ties=="secondary") daux<-monregS(diss,e,wgths)
			if (ties=="tertiary") daux<-monregT(diss,e,wgths)
			dhat<-normDiss(daux,wgths)
			}
		}
	snon<-sum(wgths*(dhat-e)^2)
	if (verbose) cat("Iteration: ",formatC(itel,width=3, format="d")," Stress: ",
		formatC(c(sold,ssma,snon),digits=8,width=12,format="f"),"\n")
	if (((sold-snon)<eps) || (itel == itmax)) break()
	x<-y; d<-e; sold<-snon; itel<-itel+1	
	}
list(dhat=dhat,d=e,x=y)
}

smacofRect<-function(
	delta,
	w=matrix(1,dim(delta)[1],dim(delta)[2]),
	p=2,
	init="svd",
	verbose=FALSE,
	itmax=100,
	reg=1e-6,
	eps=1e-6) {
n<-dim(delta)[1]; m<-dim(delta)[2]; itel<-1
delta<-ifelse(is.na(delta),0,delta)
delta_plus<-ifelse(delta>=0,delta,0); delta_min<-ifelse(delta<=0,-delta,0)
if (is.list(init))
	{
	x<-init[[1]]; y<-init[[2]]
	}
else {
	e<-delta_plus^2; e<--0.5*(e-outer(rowSums(e)/m,colSums(e)/n,"+")+(sum(e)/(n*m)))
	z<-svd(e,nu=p,nv=0); x<-z$u;  y<-crossprod(e,x)
	}
d<-distRect(x,y,reg); lold<-sum(w*(delta-d)^2)
repeat {
	ww<-w*(1+(delta_min/d)); wr<-rowSums(ww); wc<-colSums(ww)
	v<-solve(diag(wc)+(1/m)-crossprod(ww,ww/wr))-(1/m)
	b<-w*delta_plus/d; br<-rowSums(b); bc<-colSums(b)
	xraw<-(br*x)-(b%*%y); yraw<-(bc*y)-crossprod(b,x)
	y<-v%*%(yraw+crossprod(ww,xraw/wr)); x<-(xraw+(ww%*%y))/wr; d<-distRect(x,y,reg)
    lnew<-sum(w*(delta-d)^2)
	if (verbose) {
		cat("Iteration: ",formatC(itel,digits=6,width=6),
	    	"   Stress:   ",formatC(lold,digits=6,width=12,format="f"),
	    	" ==>",formatC(lnew,digits=6,width=12,format="f"),"\n")
	    }
	if (((lold-lnew)<eps) || (itel==itmax)) break()
	lold<-lnew; itel<-itel+1
	}
return(list(x=x,y=y,d=d,l=lnew))
}

#
# Chapter II: SMACOF with Constraints on the Configuration
#

smacofCons<-function(
	diss,
	constraints,
	p,
	wgths=initWeights(diss),
	metric=TRUE,
	ties="primary",
	extra=FALSE,	
	verbose=FALSE,
	modulus=1,
	itmax=100,
	eps=1e-6) {
n<-attr(diss,"Size"); m<-length(diss); dhat<-normDiss(diss,wgths)
w<-vmat(wgths); v<-myGenInv(w); itel<-1;
x<-constraints(matrix(rnorm(n*p),n,p),w,extra)
d<-dist(x); lb<-sum(wgths*d*dhat)/sum(wgths*d^2); x<-lb*x; d<-lb*d
sold<-sum(wgths*(dhat-d)^2)
repeat {
	b<-bmat(dhat,wgths,d); y<-v%*%b%*%x
	y<-constraints(y,w,extra); e<-dist(y)
	ssma<-sum(wgths*(dhat-e)^2)
	if (!metric) {
	    if ((itel%%modulus) == 0) {
			if (ties=="primary") daux<-monregP(diss,e,wgths)
			if (ties=="secondary") daux<-monregS(diss,e,wgths)
			if (ties=="tertiary") daux<-monregT(diss,e,wgths)
			dhat<-normDiss(daux,wgths)
			}
		}
	snon<-sum(wgths*(dhat-e)^2)
	if (verbose) cat("Iteration: ",formatC(itel,width=3, format="d")," Stress: ",
		formatC(c(sold,ssma,snon),digits=8,width=12,format="f"),"\n")
	if (((sold-snon)<eps) || (itel == itmax)) break()
	x<-y; d<-e; sold<-snon; itel<-itel+1	
	}
list(dhat=dhat,d=e,x=y)
}

smacofSpherical<-function(
	diss,
	p=2,
	init=FALSE,
	wgths=initWeights(diss),
	metric=TRUE,
	ties="primary",
	verbose=TRUE,
	modulus=1,
	itmax=100,
	eps=1e-6) {
n<-attr(diss,"Size"); m<-length(diss); dhat<-normDiss(diss,wgths)
w<-vmat(wgths); v<-myGenInv(w); itel<-1;
if (!is.matrix(init)) x<-torgerson(dhat,p=p) else x<-init
x<-x/sqrt(rowSums(x^2))
d<-dist(x); lb<-sum(wgths*d*dhat)/sum(wgths*d^2); x<-lb*x; d<-lb*d
sold<-sum(wgths*(dhat-d)^2)
repeat {
	b<-bmat(dhat,wgths,d); y<-v%*%b%*%x
	y<-sphereProj(y,w); e<-dist(y)
	ssma<-sum(wgths*(dhat-e)^2)
	if (!metric) {
	    if ((itel%%modulus) == 0) {
			if (ties=="primary") daux<-monregP(diss,e,wgths)
			if (ties=="secondary") daux<-monregS(diss,e,wgths)
			if (ties=="tertiary") daux<-monregT(diss,e,wgths)
			dhat<-normDiss(daux,wgths)
			}
		}
	snon<-sum(wgths*(dhat-e)^2)
	if (verbose) cat("Iteration: ",formatC(itel,width=3, format="d")," Stress: ",
		formatC(c(sold,ssma,snon),digits=8,width=12,format="f"),"\n")
	if (((sold-snon)<eps) || (itel == itmax)) break()
	x<-y; d<-e; sold<-snon; itel<-itel+1	
	}
list(dhat=dhat,d=e,x=y)
}

smacofDouble<-function(
	diss,
	penalty,
	wgths=initWeights(diss),
	p=2,
	init="none",
	sphere=TRUE,
	metric=TRUE,
	ties="primary",
	verbose=FALSE,
	relax=1,
	modulus=1,
	itmax=100,
	eps=1e-6) {
n<-attr(diss,"Size"); dhat<-normDiss(diss,wgths)
if (!is.matrix(init)) x<-torgerson(dhat,p=p) else x<-init
if (sphere) {
	mn<-c(1,rep(0,n))
	dhat1<-as.dist(rbind(0,cbind(0,as.matrix(dhat))))
	diss<-as.dist(rbind(0,cbind(0,as.matrix(diss))))
	wgths1<-as.dist(rbind(0,cbind(0,as.matrix(wgths))))
	wgths2<-as.dist(outer(mn,mn,function(x,y) abs(x-y)))
	dhat2<-mean(sqrt(rowSums(x^2)))*wgths2
	x<-rbind(0,x)
}
if (!sphere) {
	mn<-c(1,rep(0,n))
	dhat1<-as.dist(rbind(0,0,cbind(0,0,as.matrix(dhat))))
	diss<-as.dist(rbind(0,0,cbind(0,0,as.matrix(diss))))
	wgths1<-as.dist(rbind(0,0,cbind(0,0,as.matrix(wgths))))
	wgths2<-as.dist(outer(mn,mn,function(x,y) abs(x-y)))
	dhat2<-mean(sqrt(rowSums(x^2)))*wgths2
	x<-rbind(0,x)
}
w<-vmat(wgths1+penalty*wgths2); v<-myGenInv(w); itel<-1;
d<-dist(x); lb<-sum(wgths1*d*dhat1)/sum(wgths1*d^2); x<-lb*x; d<-lb*d
sold1<-sum(wgths1*(dhat1-d)^2); sold2<-sum(wgths2*(dhat2-d)^2)
sold<-sold1+penalty*sold2
repeat {
	b<-bmat(dhat1,wgths1,d)+penalty*bmat(dhat2,wgths2,d); y<-v%*%b%*%x
	y<-x+relax*(y-x); e<-dist(y)
	ssma1<-sum(wgths1*(dhat1-e)^2); ssma2<-sum(wgths2*(dhat2-e)^2)
	ssma<-ssma1+penalty*ssma2
	if ((!metric) && ((itel%%modulus) == 0)) {
		if (ties=="primary") daux<-monregP(diss,e,wgths1)
		if (ties=="secondary") daux<-monregS(diss,e,wgths1)
		if (ties=="tertiary") daux<-monregT(diss,e,wgths1)
		dhat1<-normDiss(daux,wgths1)
		}
	if (sphere) dhat2<-mean(e[1:n])*wgths2
	snon1<-sum(wgths1*(dhat1-e)^2); snon2<-sum(wgths2*(dhat2-e)^2)
	snon<-snon1+penalty*snon2
	if (verbose) cat("Iteration: ",formatC(itel,width=3, format="d"),"\n",
		" StressOld: ",formatC(c(sold1,sold2,sold),digits=8,width=12,format="f"),"\n",
		" StressSma: ",formatC(c(ssma1,ssma2,ssma),digits=8,width=12,format="f"),"\n",
		" StressNon: ",formatC(c(snon1,snon2,snon),digits=8,width=12,format="f"),"\n",
		"\n\n")
	if (((sold-snon)<eps) || (itel == itmax)) break()
	x<-y; d<-e; sold<-snon; itel<-itel+1	
	}
list(dhat1=dhat1,dhat2=dhat2,d=e,x=y)
}

#
# Chapter III: SMACOF for Three-Way Data
#

smacofIndDiff<-function(
	diss,
	wgths=initWeights(diss),
	p=2,
	init="torg",
	metric=TRUE,
	ties="primary",
	constr="null",
	verbose=FALSE,
	modulus=1,
	itmax=100,
	eps=1e-6) {
if (!is.list(diss)) diss<-list(diss); if (!is.list(wgths)) wgths<-list(wgths)
n<-attr(diss[[1]],"Size"); m<-length(diss); itel<-1
dr<-list(); wr<-list(); vr<-list(); dh<-list()
for (j in 1:m) {
	wr<-appendList(wr,vmat(wgths[[j]]))
	vr<-appendList(vr,myGenInv(wr[[j]]))
	dh<-appendList(dh,normDiss(diss[[j]],wgths[[j]]))
	}
xr<-list(); sold<-sf1<-sf2<-0
if (init=="torg") aconf<-torgerson(sumList(diss),p) 
	else aconf<-matrix(rnorm(n*p),n,p)
bconf<-repList(diag(p),m)
for (j in 1:m) {
	xr[[j]]<-aconf%*%bconf[[j]]
	dr[[j]]<-dist(xr[[j]]); sf1<-sf1+sum(wgths[[j]]*dr[[j]]*dh[[j]])
	sf2<-sf2+sum(wgths[[j]]*dr[[j]]^2)
	}
lb<-sf1/sf2
for (j in 1:m) {
	aconf<-lb*aconf
	xr[[j]]<-lb*xr[[j]]; dr[[j]]<-lb*dr[[j]]
	sold<-sold+sum(wgths[[j]]*(dh[[j]]-dr[[j]])^2)
	}
repeat {
    br<-list(); yr<-list(); er<-list(); sunc<-0
	for (j in 1:m) {
		br<-appendList(br,bmat(dh[[j]],wgths[[j]],dr[[j]]))
		yr<-appendList(yr,vr[[j]]%*%br[[j]]%*%xr[[j]])
		er<-appendList(er,dist(yr[[j]]))
		sunc<-sunc+sum(wgths[[j]]*(dh[[j]]-er[[j]])^2)
		}
	scon<-sunc
	if (constr != "null") {
		scon<-0; er<-list()
		if (constr=="ident") {
			z<-matrix(0,n,p); u<-matrix(0,n,n)
			for (j in 1:m) {
				z<-z+wr[[j]]%*%yr[[j]]
				u<-u+wr[[j]]
				}
			aconf<-myGenInv(u)%*%z; yr<-repList(aconf,m)
			}
		if (constr=="diag") {
			aux0<-matrix(0,n,p)
			for (j in 1:m) {
				aux1<-diag(crossprod(aconf,wr[[j]]%*%yr[[j]]))
				aux2<-diag(crossprod(aconf,wr[[j]]%*%aconf))
				bconf[[j]]<-diag(aux1/aux2)
				aux0<-aux0+(wr[[j]]%*%yr[[j]]%*%bconf[[j]])		    			    
				}
			for (s in 1:p) {
				aux1<-matrix(0,n,n)
				for (j in 1:m) 
					aux1<-aux1+(bconf[[j]][s,s]^2)*wr[[j]]
				aconf[,s]<-myGenInv(aux1)%*%aux0[,s]
				}
			for (j in 1:m)
				yr[[j]]<-aconf%*%bconf[[j]]
			}
		if (constr=="idio") {
			aux0<-matrix(0,n,p); auxk<-matrix(0,(n*p),(n*p))
			for (j in 1:m) {
				aux1<-crossprod(aconf,wr[[j]]%*%yr[[j]])
				aux2<-crossprod(aconf,wr[[j]]%*%aconf)
				auxb<-solve(aux2,aux1); bconf[[j]]<-auxb
				auxc<-crossprod(t(auxb))
				aux0<-aux0+(wr[[j]]%*%yr[[j]]%*%t(auxb))	
				auxk<-auxk+kronecker(auxc,wr[[j]])    			    
				}
			auxv<-kronecker(diag(p),matrix((1/n),n,n))
		    aconf<-matrix(solve(auxk+auxv,as.vector(aux0)),n,p)
			for (j in 1:m)
				yr[[j]]<-aconf%*%bconf[[j]]
			}
		for (j in 1:m) {
			er<-appendList(er,dist(yr[[j]]))
			scon<-scon+sum(wgths[[j]]*(dh[[j]]-er[[j]])^2)
			}
		}
	snon<-scon
	if (!metric) {
	    if ((itel%%modulus) == 0) {
			snon<-0; dh<-list()
			for(j in 1:m) {
				ds<-diss[[j]]; es<-er[[j]]; ws<-wgths[[j]]
				if (ties=="primary") do<-monregP(ds,es,ws)
				if (ties=="secondary") do<-monregS(ds,es,ws)
				if (ties=="tertiary") do<-monregT(ds,es,ws)
				dh<-appendList(dh,normDiss(do,ws))
				snon<-snon+sum(ws*(dh[[j]]-es)^2)
				}
			}
		}
	if (verbose) cat("Iteration: ",formatC(itel,width=3, format="d")," Stress: ",
		formatC(c(sold,sunc,scon,snon),digits=8,width=12,format="f"),"\n")
	if (((sold-snon)<eps) || (itel == itmax)) break()
	xr<-yr; dr<-er; sold<-snon; itel<-itel+1
	}
list(dh=dh,dr=er,xr=yr,a=aconf,b=bconf)
}

uniqueC<-function(x,w,extra) {
n<-dim(x)[1]; p<-dim(x)[2]-n
return(cbind(x[,1:p],diag(diag(w%*%x[,p+(1:n)])/diag(w))))
}

linearC<-function(x,w,extra) {
return(extra%*%solve(crossprod(extra,w%*%extra),crossprod(extra,w%*%x)))
}

diagC<-function(x,w,extra) {
return(extra%*%diag(colSums(extra*(w%*%x))/colSums(extra*(w%*%extra))))
}

sphereProj<-function(y,v,init=FALSE,immediate=FALSE,ktmax=100,itmax=5,oeps=1e-10,ieps=1e-6,iverbose=FALSE,overbose=FALSE) {
	require("polynom"); n<-nrow(y); nn<-1:n; itel<-1
	u<-v%*%y; syy<-sum(y*u); tau<-rowSums(u^2)
	if (is.matrix(init)) z<-init else z<-y/sqrt(rowSums(y^2))
	repeat {
		szy<-sum(z*u); vz<-v%*%z; szz<-sum(z*vz); rho<-(szy^2)/szz
		if (overbose) cat("Iteration: ",formatC(itel,digits=3,width=3),
							"Rho: ",formatC(rho,digits=6,width=10,format="f"),
							"\n")
			kmax<-1
			repeat {		
				for (i in nn) {
				    ui<-u[i,]; zi<-z[i,]; vz<-v%*%z; vi<-vz[i,]; tti<-tau[i]; szy<-sum(z*u) 
					if (immediate) {
						szz<-sum(z*vz)
						rho<-(szy^2)/szz
						}
					hi<-ui*(szy-sum(ui*zi))-rho*(vi-v[i,i]*zi)
					sold<-(sum(ui*zi)^2)+2*sum(hi*zi)
					ppi<-(sum(ui*hi)^2)/tti; qqi<-sum(hi^2)-ppi
					p0<-qqi*tti^2; p1<--2*tti*qqi; p2<-ppi+qqi-tti^2; p3<-2*tti; p4<--1
					pol<-polynomial(c(p0,p1,p2,p3,p4)); the<-max(Re(solve(pol)))
					zi<-(hi/the)-((1/the)+(1/(tti-the)))*(sum(ui*hi)/tti)*ui; z[i,]<-zi
					snew<-(sum(ui*zi)^2)+2*sum(hi*zi)
					if (iverbose) cat("Iteration: ",formatC(itel,digits=3,width=3),
										"sol: ",formatC(sold,digits=6,width=10,format="f"),
										"sne: ",formatC(snew,digits=6,width=10,format="f"),
										"\n")
						
					}			
				if ((kmax == ktmax) || (ieps<0)) break()
				kmax<-kmax+1
			}
		if ((itel == itmax) || (oeps<0)) return((szy/szz)*z)
		itel<-itel+1
	}
}