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