# Test points 
x=c(-5,-4,-3,-2,-1,0,1,2,3,4,5,6,7)
y=c(-9,-4,-6,-3,-2,-1,2,3,5,3,7,4,8)

plot(x,y)

#1. Ordinary least square --------------------------------------------------------------------------
# If p(x)=a+bx
# Let A=[x_i^(j-1)] and u=(a,b). We need to minimize (Au-y)^*(Au-y).
# with no constraining
m=length(x)
n=2

A=matrix(0,m,n) 
for (i in 1:m){
for (j in 1:n){A[i,j]=x[i]^(j-1)}
}

## Solves linear equation A^*(Au-y)=0
# 

ur=solve(t(A)%*%A,t(A)%*%y)
print('Ordinary least square')
t(ur)
print("Residue")
t((A%*%ur-y))%*%(A%*%ur-y)
poli<-function(t){
p=ur[1]
for (i in 1:(n-1)){p=p+ur[i+1]*t^i}
return(p)
}

t=seq(min(x),max(x),by=0.1);
z=c(poli(t[1]))
for ( i in 2:length(t)){z[i]=c(poli(t[i]))}
plot(t,z,'l',ylim=c(min(y),max(y)), main="Ordinary least square fit")
points(x,y)

# Constraining Least square ------------------------------------------------------------------------
# If p(x)=a+bx
# Let A=[x_i^(j-1)] and u=(a,b). We need to minimize (Au-y)^*(Au-y).
# Let us constraining by p(x)>=f(x) which means, in this case that Au>=y.
u0=c(3,1)  #p(x)=x+100 is an initial guess.
m=length(x)
n=length(u0)

A=matrix(0,m,n) 
for (i in 1:m){
for (j in 1:n){A[i,j]=x[i]^(j-1)}
}

## Solves linear and quadratic programming problems
## but needs a feasible starting value
#
# from example(solve.QP) in 'quadprog'
# 
fQP <- function(u) {t(A%*%u-y)%*%(A%*%u-y)}
Amat       <- A
bvec       <- y
# derivative
gQP <- function(u) {2*t(A)%*%(A%*%u-y)}
result=constrOptim(u0, fQP, gQP, ui = A, ci = bvec)
print('Constraining  least square')
result$par
print("Residue")
result$value
ur=result$par

poli<-function(t){
p=ur[1]
for (i in 1:(n-1)){p=p+ur[i+1]*t^i}
return(p)
}

t=seq(min(x),max(x),by=0.1);
z=c(poli(t[1]))
for ( i in 2:length(t)){z[i]=c(poli(t[i]))}
plot(t,z,'l',ylim=c(min(y),max(y)),main="Constraining least square fit")
points(x,y)

# Norm max ------------------------------------------------------------------------
# If p(x)=a+bx
# Let A=[x_i^(j-1)] and u=(a,b). We need to minimize |p(x_)-y_i|.
# Let us constraining by min r, with r>=Au-y and r>=y-Au.
u0=c(100,1,1000)  #p(x)=x+100 is an initial guess.
m=length(x)
n=length(u0)-1

A=matrix(0,m,n); Id=0*(1:(2*m))
for (i in 1:m){
for (j in 1:n){A[i,j]=x[i]^(j-1)}
Id[i]=1
Id[m+i]=1
}

B=matrix(0,2*m,n+1)
B[1:m,1:n]=-A
B[(m+1):(2*m),1:n]=A
B[,n+1]=Id

yy=Id
yy[1:m]=-y
yy[(m+1):(2*m)]=y
## Solves linear and quadratic programming problems
## but needs a feasible starting value
#
# from example(solve.QP) in 'quadprog'
# 
fQP <- function(u) {u[length(u)]}
Amat       <- B
bvec       <- yy

gQP <- function(u) {v=0*(1:length(u))
    v[length(u)]=1
    v
}
####
result=constrOptim(u0, fQP,gQP, ui = B, ci = bvec)

ur=result$par
print('Norm max')
result$par[1:2]
print("Residue")
result$value

poli<-function(t){
p=ur[1]
for (i in 1:(n-1)){p=p+ur[i+1]*t^i}
return(p)
}

t=seq(min(x),max(x),by=0.1);
z=c(poli(t[1]))
for ( i in 2:length(t)){z[i]=c(poli(t[i]))}
plot(t,z,'l',ylim=c(min(y),max(y)),main="Max norm fit")
points(x,y)