#!/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)