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