#!/usr/bin/python
# 14.2
import os, sys
sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"]

import scipy as sp
from scipy import linalg
from pend import cont_mat, delta_a, acker
from polyfuncs import polmul

A = sp.array([[1, 2], [0, 3]])
c = sp.array([1, 0])
p = sp.roots([1, -0.5, 0.5])
b = sp.array([[0], [1]])
K = acker(A, b, p) 

p1 = 0.1 + 0.1j
p2 = 0.1 - 0.1j
phi = sp.real(sp.convolve([1, -p1], [1, -p2]))
Obs = sp.vstack((c, sp.dot(c, A)))
alphae = sp.dot(A, A)-0.2*A+0.02*sp.eye(2,2)
Lp = sp.dot(alphae, sp.dot(linalg.inv(Obs), sp.array([[0],[1]])))

C = sp.array([[1, 0, 0.5, 2], [0, 1, -4.71, 2.8]])
dC = 1

[HD,dHD] = polmul(K,0,C,dC)
[HD,dHD] = polmul(HD,dHD,Lp,0)

print HD, dHD