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