#Simple Conjugate Gradient in 5d

#g(x) = (1/2) x'Ax - b'x + c

p<-5
A<-rWishart(1,p+1,diag(p))[,,1]
b<-rnorm(p)
cval<--3

xstar<-solve(A)%*%b




#############################################################
#Recursion: Initial approach

x.old<-rnorm(p)*5
r.old<-b - A %*% x.old
alpha.old<-(t(r.old) %*% r.old/(t(r.old) %*% A %*% r.old))[1,1]
sse.old<-sum(r.old^2)

step.vals<-c(x.old,r.old,alpha.old,sse.old)

abstol<-1e-16

while(sse.old > abstol){

	xcur<-x.old
	x.old<-x.old+alpha.old*r.old
	r.old<-b - A %*% x.old
	alpha.old<-(t(r.old) %*% r.old/(t(r.old) %*% A %*% r.old))[1,1]
	sse.old<-sum(r.old^2)
	steps.val<-rbind(step.vals,c(x.old,r.old,alpha.old,sse.old))

}

#############################################################
#Recursion: Orthogonal approach

x.old<-rnorm(p)
r.old<-b - A %*% x.old
d.old<-r.old
alpha.old<-(t(r.old) %*% r.old/(t(d.old) %*% A %*% d.old))[1,1]
sse.old<-sum(r.old^2)

step.vals<-c(x.old,r.old,d.old,alpha.old,sse.old)

print(c(0,sse.old))
for(istep in 1:10){

	xcur<-x.old
	rcur<-r.old
	dcur<-d.old
	x.old<-x.old+alpha.old*d.old
	r.old<-rcur - alpha.old * A %*% d.old
	d.old<-r.old+((t(r.old) %*% r.old)/(t(rcur) %*% rcur))[1,1] * dcur
	sse.old<-sum(r.old^2)
	alpha.old<-((t(r.old) %*% r.old)/(t(d.old) %*% A %*% d.old))[1,1]
	step.vals<-rbind(step.vals,c(x.old,r.old,d.old,alpha.old,sse.old))
	print(c(istep,sse.old))
}