#!/usr/bin/python
# 10.9
import os, sys
sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"]

import scipy as sp
from imcsplit import imcsplit
from polyfuncs import poladd
from zpowk import zpowk

def imc_stable(B, A, k, alpha):
    """ Designs Discrete Internal Model Controller
    for transfer function z^{-k}B(z^{-1})/A(z^{-1})
    Numerator and Denominator of IMC HQ are outputs
    Controller is also given in R,S form. """
    Kp, d, Bg, Bnmp, Bm = imcsplit(B, 1)
    Bg = Kp * Bg
    Bnmpr = Bnmp[::-1]
    Bms = sum(Bm)
    HiN = A
    HiD = Bms * sp.convolve(Bg, Bnmpr)
    k = k+d

    zk, dzk = zpowk(k)
    zk = sp.squeeze(zk)
    Bf = 1-alpha
    Af = sp.array([1, -alpha])
    S = sp.convolve(Bf, A)
    R1 = sp.convolve(Af, sp.convolve(Bnmpr, Bms))
    R2 = sp.convolve(zk, sp.convolve(Bf, sp.convolve(Bnmp, Bm)))

    R, dR = poladd(R1, len(R1)-1, -R2, len(R2)-1)
    R = sp.squeeze(R)
    R = sp.convolve(Bg, R)

    return k, HiN, HiD, R, S