import numpy as np
import matplotlib.pyplot as plt
%matplotlib inline

np.random.seed(2)
X=np.arange(0,1.05,0.05)
y=np.random.randn(21)
cnum=2
constr_X = np.array([0.0,1])
constr_Y = np.array([0,0])

d_cnum=2
dconstr_X = np.array([0.0,1])
dconstr_Y = np.array([-3,3])

deg = 6
poly = np.zeros((X.shape[0],deg))
constr = np.zeros((constr_X.shape[0], deg))
dconstr = np.zeros((dconstr_X.shape[0], deg))
for i in range(deg):
    poly[:,i] = X**(i)
for i in range(deg):
    constr[:,i] = constr_X**(i)
for i in range(1,deg):
    dconstr[:,i] = i*dconstr_X**(i-1)

XX = np.dot(poly.T, poly)
Xy =  np.dot(poly.T, y)
constr
total_A = np.zeros((deg+cnum+d_cnum,deg +cnum +d_cnum))
total_A[:deg,:deg] = XX
total_A[deg:deg+cnum,:deg ] = constr
total_A[:deg ,deg:deg+cnum] = constr.T
total_A[deg+cnum:,:deg ] = dconstr
total_A[:deg,deg+cnum:] = dconstr.T

total_B = np.zeros(deg+cnum + d_cnum)
total_B[:deg] = Xy
total_B[deg:deg+cnum] = constr_Y
total_B[deg+cnum:deg+cnum+d_cnum] = dconstr_Y

solved = np.linalg.solve(total_A, total_B)[:deg]
plt.scatter(X,y)
plt.plot(X, np.dot(poly,solved))