#!/usr/bin/python
# 13.4

import os, sys
sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"]

import scipy as sp
from polyfuncs import polmul, putin, ext
from xdync import xdync
from zpowk import zpowk
from specfac import specfac

def lqg_simple(A,dA,B,dB,C,dC,k,rho,V,dV,W,dW,F,dF): 
    """ LQG controller design by method of Ahlen and Sternad. """
    r, b, db = specfac(A,dA,B,dB,rho,V,dV,W,dW,F,dF)
    D, dD = polmul(A, dA, F, dF)
    zk, dzk = zpowk(k)
    N, dN = polmul(zk, dzk, B, dB)
    RHS, dRHS = polmul(C, dC, b, db)
    S, dS, R1, dR1 = xdync(N, dN, D, dD, RHS, dRHS)[:4]
    return R1, dR1, S, dS

if __name__ == "__main__":
    A, dA = sp.array([1, -0.44]), 1
    B, dB = sp.array([0.51, 1.21]), 1
    F, dF = sp.array([1, -1]), 1
    C, dC = sp.array([1, -0.44]),1
    k = 1
    V, W = 1, 1
    dV, dW = 0, 0
    rho = 1
    int1 = 1

    print lqg_simple(A,dA,B,dB,C,dC,k,rho,V,dV,W,dW,F,dF)