set.seed(2348)
n<-500
p<-100
true.beta<-c(5,rep(c(2,0),c(5,p-5)))
true.Sigma<-rWishart(1,p+10,diag(p)/(p+10))[,,1]
library(MASS)
Xmat<-mvrnorm(n,mu=rep(0,p),Sigma=true.Sigma)

ssq<-apply(Xmat^2,2,sum)

X<-cbind(rep(1,p),Xmat)

Y<-X%*%true.beta+rnorm(n)

beta.hat<-coef(lm(Y~Xmat))

old.mu<-beta.hat[1]
old.beta<-beta.hat[-1]
old.res<-Y-X%*%beta.hat

lamvec<-seq(1,100,by=0.5)
coef.mat<-matrix(0,ncol=length(lamvec),nrow=p+1)
BIC.vec<-min.func<-nc.vec<-rep(0,length(lamvec))
for(i in 1:length(lamvec)){
	lambda<-lamvec[i]
	tolerance<-1e-12
	change.beta<-Inf
	iter<-0
	obj.val<-sum(old.res^2)+lambda*sum(abs(old.beta))
	while(change.beta > tolerance){

		iter<-iter+1
		old.vec<-c(old.mu,old.beta)

		new.mu<-old.mu+mean(old.res)
		old.res<-old.res+(old.mu-new.mu)
		old.mu<-new.mu

		for(k in 1:p){
			new.beta1<-min(0,old.beta[k]-(-sum(old.res*Xmat[,k])-lambda)/ssq[k])
			new.beta2<-max(0,old.beta[k]-(-sum(old.res*Xmat[,k])+lambda)/ssq[k])
			new.beta<-new.beta1+new.beta2
			old.res<-old.res+Xmat[,k]*(old.beta[k]-new.beta)
			old.beta[k]<-new.beta
		}
		new.vec<-c(old.mu,old.beta)
		change.beta<-sum((old.vec-new.vec)^2)

		obj.val<-sum(old.res^2)+lambda*sum(abs(old.beta))
	
	}
	min.func[i]<-obj.val
	coef.mat[,i]<-new.vec
	nc.vec[i]<-sum(new.vec[-1]!=0)
	BIC.vec[i]<-sum(old.res^2)+nc.vec[i]*log(n)
	print(c(i,iter,lamvec[i],obj.val,BIC.vec[i],nc.vec[i]))
}

plot(lamvec,BIC.vec,type='l',ylab='BIC',xlab=expression(lambda))