#!/usr/bin/python

# 13.3

import os, sys

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

import scipy as sp

from polyfuncs import polmul, poladd

def specfac(A, dA, B, dB, rho, V, dV, W, dW, F, dF):

    """Implements the spectral factorization for use with LQG 

    control by design method of Ahlen and Sternad"""

    AFW = sp.convolve(A, sp.convolve(W, F))

    dAFW = dA + dF + dW

    AFWWFA = rho * sp.convolve(AFW, AFW[::-1])

    BV = sp.convolve(B, V)

    dBV = dB + dV

    BVVB = sp.convolve(BV, BV[::-1])

    diff = dAFW - dBV

    dBVVB = 2*dBV

    for i in range(diff):

        BVVB, dBVVB = polmul(BVVB, dBVVB, sp.array([0, 1]), 1)

    rbb, drbb = poladd(AFWWFA, 2*dAFW, BVVB, dBVVB)

    rbb = rbb.squeeze()

    rts = sp.roots(rbb)

    rtsin = rts[dAFW:2*dAFW+1]

    b = 1

    for i in range(dAFW):

        b = sp.convolve(b, sp.array([1, -rtsin[i]]))

    b = sp.real(b)

    bbr = sp.convolve(b, b[::-1])

    r = rbb[0] / bbr[0]

    return r, b, dAFW

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

    V, W = 1, 1

    dV, dW = 0, 0

    rho = 1

    int1 = 1

    print specfac(A,dA,B,dB,rho,V,dV,W,dW,F,dF)