#!/usr/bin/python

import sys
sys.path += ['/media/all/work/digital_control/pycode/python/']

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

def lqg_as(A,degA,B,degB,C,degC,k,rho,V,degV,W,degW,F,degF): 
    """ LQG controller design by method of Ahlen and Sternad. """
    r, b, degb = specfac(A,degA,B,degB,rho,V,degV,W,degW,F,degF)
    WFA = sp.convolve(A, sp.convolve(F,W))[::-1]
    dWFA = degW + degF + degA
    rhs1, drhs1 = polmul(W, degW, WFA, dWFA)
    print rhs1, "rhs1", drhs1
    print C, "C", degC
    rhs1, drhs1 = polmul(rhs1, drhs1, C, degC)
    rhs1 = rho * rhs1
    rhs2 = sp.convolve(C, sp.convolve(V, sp.convolve(B,V)[::-1]))
    drhs2 = degC + 2*degV + degB
    for i in range(degb - degB - degV):
        rhs2 = sp.convolve(rhs2, [0,1])
    drhs2 = drhs2 + degb - degB - degV
    C1 = sp.zeros((1,2))

    C1, degC1 = putin(C1,0,rhs1,drhs1,0,0)
    C1, degC1 = putin(C1,degC1,rhs2,drhs2,0,1)
    rbf = r * b[::-1]
    D1 = sp.zeros((2, 2))
    D1, degD1 = putin(D1,0,rbf,degb,0,0)
    for i in range(k):
        rbf = sp.convolve(rbf, [0, 1])
    D1, degD1 = putin(D1,degD1,rbf,degb+k,1,1)
    N = sp.zeros((1, 2))
    N, degN = putin(N,0,-B,degB,0,0)
    
    AF, dAF = polmul(A,degA,F,degF)
    N, degN = putin(N,degN,AF,dAF,0,1)

    Y, degY, X, degX = xdync(N, degN, D1, degD1, C1, degC1)[:4]

    R, degR = ext(X,degX,0,0)
    S, degS = ext(X,degX,0,1)
    X = Y[::-1]

    return R, degR, S, degS

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_as(A,dA,B,dB,C,dC,k,rho,V,dV,W,dW,F,dF)