#!/usr/bin/env python

# 12.11

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, poladd

def gpc_pid(A, dA, B, dB, C, dC, N1, N2, Nu, lambda1, gamm, gamma_y):

    Adelta = pl.convolve(A,[1, -1])

    G = pl.array([])

    Gtilda1 = pl.array([])

    for i in range(N1, N2+1):

        zi=zpowk(i)[0]

        E, dE, F, dF = xdync(Adelta, dA+1, zi, i, C, dC)[:4]

        Gtilda, dGtilda, Gbar, dGbar = xdync(C, dC, zi, i, E*B, dE+dB)[:4]

        Gtilda = pl.atleast_1d(Gtilda.squeeze())

        Gtilda1 = pl.empty(i)

        for j in range(i): 

            Gtilda1[j] = Gtilda[i-j-1]

        if i <= Nu-1:

            Gtilda2 = pl.zeros(Nu)

            Gtilda2[:i] = Gtilda1

            if pl.size(G) is not 0:

                G = pl.vstack((G, Gtilda2))

            else:

                G = Gtilda2

        else:

            G = pl.vstack((G, Gtilda1[:Nu]))

    es=sum(pl.atleast_1d(C))/sum(pl.atleast_1d(A))

    gs=sum(pl.atleast_1d(B))/sum(pl.atleast_1d(A))

    F_s=es * A

    G_s=[]

    for i in range(1, Nu+1):

        if (Nu-i) == 0:

            row= gs * pl.ones(i)

        else:

            row = gs * pl.ones(i)

            row = pl.column_stack((row, pl.zeros((Nu-i,Nu-i))))

            row = row.squeeze()

        if pl.size(G_s) is not 0:

            G_s = pl.row_stack((G_s, row))

        else:

            G_s = row

    lambda_mat = lambda1 * pl.identity(Nu)

    gamma_mat = gamm * pl.identity(Nu)

    gamma_y_mat = gamma_y * pl.identity(N2-N1+1)

    mat1 = pl.inv(pl.dot(G.T, pl.dot(gamma_y_mat, G))+lambda_mat+pl.dot(G_s.T, pl.dot(gamma_mat,G_s)))

    mat2 = pl.dot(mat1, pl.dot(G.T, gamma_y_mat))

    mat2_s = pl.dot(mat1, pl.dot(G_s.T, gamma_mat))

    h_s= sum(mat2_s[0,:])

    h = mat2[0,:]

    T = C 

    R = C * (sum(h)+h_s)

    S = 0

    for i in range (N1, N2+1):

        zi=zpowk(i)[0]

        E, dE, F, dF = xdync(Adelta, dA+1, zi, i, C, dC)[:4]

        Gtilda, dGtilda, Gbar, dGbar = xdync(C, dC, zi, i, E*B, dE+dB)[:4]

        S += F*h[i-1]

    S += F_s*h_s

    S = S.squeeze()

    if len(A) == 3:

        Kp = S[0] - R - S[2]

        Ki = R 

        Kd = S[2]

    else:

        Kp = S[1] - R

        Ki = R 

        Kd = 0

    return Kp, Ki, Kd