#  code to fit scatterplot with model
#  y=Xb+e, using constrained least-squares,
#  where the constraints are in the form Ab>=0,
#  where A is irreducible.
#  Also does the test of null: Ab=0 vs Ab>0 
#
#  uses mix-of-betas null distribution
#  returns p-value, constrained and unconstrained fits.

constrp=function(xmat,y,amat){
	n=length(y)
	sm=1e-10
	m=length(xmat)/n
	pmat=xmat%*%solve(t(xmat)%*%xmat)%*%t(xmat)
	umat=chol(t(xmat)%*%xmat)
	uinv=solve(umat)
	atil=amat%*%uinv
	z=t(uinv)%*%t(xmat)%*%y
	ans=coneproj(z,atil)
	bhat=uinv%*%ans$phihat
	fhatc=xmat%*%bhat
	fhatuc=pmat%*%y
	ansq=qrdecomp(t(atil))
	atilq=ansq$qm
	z0=z-atilq%*%t(atilq)%*%z
	yhat0=xmat%*%uinv%*%z0
	sse0=sum((y-yhat0)^2)
	sse1=sum((y-fhatc)^2)
	bval=(sse0-sse1)/sse0
	g=qr(atil)
	dim0=m-g$rank
	if(bval>sm){
# find mixing distribution for test
		nloop=100000
		mdist=0:m*0
		for(iloop in 1:nloop){
			ys=rnorm(n)
			z=t(uinv)%*%t(xmat)%*%ys
			ans=coneproj(z,atil)
			l=ans$dim+1
			mdist[l]=mdist[l]+1
		}
		mdist=mdist/nloop
# pvalue comes from mixture of betas null distribution
		pval = sum(mdist[1:(dim0+1)])
		for(j in (dim0+2):(m+1)){
			alp=(j-dim0-1)/2; bet=(n-j+1)/2
			addl=pbeta(bval,alp,bet)*mdist[j]
			pval = pval+addl
		}
		pval=1-pval
	}else{pval=1}
	ans=new.env()
	ans$fhatc=fhatc
	ans$fhatuc=fhatuc
	ans$pval=pval
	ans
}

#################################################
coneproj=function(y,amat){
	n=length(y);m=length(amat)/n
	sm=1e-8;h=1:m<0;obs=1:m;check=0
	delta=-amat;b2=delta%*%y
	if(max(b2)>sm){
		i=min(obs[b2==max(b2)])
		h[i]=TRUE
	}else{check=1;theta=1:n*0}
	while(check==0){
		xmat=matrix(delta[h,],ncol=n)
		a=solve(xmat%*%t(xmat))%*%xmat%*%y
		if(min(a)<(-sm)){
			avec=1:m*0;avec[h]=a
			i=min(obs[avec==min(avec)])
			h[i]=FALSE;check=0
		}else{
			check=1
			theta=t(xmat)%*%a
			b2=delta%*%(y-theta)
			if(max(b2)>sm){
				i=min(obs[b2==max(b2)])		
				h[i]=TRUE;check=0
			}
		}
	}
	ans=new.env()
	ans$dim=n-sum(h)
	ans$phihat=y-theta
	ans
}
##############################################################
# computes Q of QR decomposition; returns col rank of amat
##############################################################

qrdecomp=function(xm){
	n=length(xm[,1]);m=length(xm[1,])
	nq=1
	lv=0
	qm=xm
	i=0
	while(lv==0&i<=m){
		i=i+1
		lv=sum(xm[,i]^2)
	}
	if(lv==0){stop}
	qm[,nq]=xm[,i]/sqrt(lv)
	for(j in (i+1):m){
		res=xm[,j]
		for(k in 1:nq){
			res=res-sum(qm[,k]*xm[,j])*qm[,k]
		}
		lv=sum(res^2)
		if(lv>sm){
			nq=nq+1
			qm[,nq]=res/sqrt(lv)
		}
	}
	qm=qm[,1:nq]
	ans=new.env()
	ans$qm=qm
	ans$rank=nq
	ans
}