#!/usr/bin/env python
# 11.17

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

import pylab as pl
from xdync import xdync
from zpowk import zpowk
from polyfuncs import polmul

def gmvc_pid(A, B, k, T, Ts):
    A = pl.atleast_1d(A)
    B = pl.atleast_1d(B)
    dA, dB = len(pl.atleast_1d(A)) - 1, len(pl.atleast_1d(B)) - 1
    dT = len(T) - 1
    if dA > 2:
        print 'degree of A cannot be more than 2'
        exit(1)
    elif dB>1:
        print 'degree of B cannot be more than 1'
        exit(1)
    delta, ddelta = pl.array([1, -1]), 1
    Adelta, dAdelta = polmul(A, dA, delta, ddelta)
    Q, dQ, P, dP = xdync(Adelta, dAdelta, B, dB, T, dT)[:4]
    P = pl.atleast_1d(P.squeeze())
    PAdelta = P[0]*Adelta

    zk, dzk = zpowk(k)
    E, degE, F, degF = xdync(PAdelta, dAdelta, zk, dzk, P, dP)[:4]
    E = pl.atleast_1d(E.squeeze())
    F = pl.atleast_1d(F.squeeze())
    nu = P[0]*E[0]*B[0]
    Kc = -1/nu*(F[1]+2*F[2])
    tau_i = -(F[1]+2*F[2])/(F[0]+F[1]+F[2])*Ts
    tau_d = -F[2]/(F[1]+2*F[2])*Ts
    L = pl.empty(3)
    L[0] = 1+Ts/tau_i+tau_d/Ts
    L[1] = -(1+2*tau_d/Ts)
    L[2] = tau_d/Ts
    L = Kc * L.T

    return Kc, tau_i, tau_d, L