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