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