#!/usr/bin/python

# 14.1

# Uses A, B from 2.1 (pend_model.py)

import os, sys

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

import scipy as sp

from scipy import signal

from scipy import linalg

from dscr import dscr

from pend_model import pend_model

def pol2cart(theta, r):

    x = r*sp.cos(theta)

    y = r*sp.sin(theta)

    return x, y

def cont_mat(a, b):

    n = len(a)

    con = b

    for i in range(1, n):

        prod = sp.identity(n)

        for j in range(i):

            prod = sp.dot(prod, a)

        prod = sp.dot(prod, b)

        con = sp.hstack((con, prod))

    return con

def delta_a(a, P):

    n = len(a)

    delta_a = sp.identity(n)

    for pole in P:

        delta_a = sp.dot(delta_a, (a - pole*sp.identity(n)))

    return delta_a

def acker(a, b, P):

    """ Crude implementation of ackermann's formula """

    con = cont_mat(a, b)

    con_inv = linalg.inv(con)

    del_a = delta_a(a, P)

    e_n = sp.zeros(len(a))

    e_n[-1] = 1

    K = sp.dot(sp.dot(e_n, con_inv), del_a)

    return K

if __name__ == "__main__":

    A, B = pend_model()

    # Changed form of C, D from the scilab one to make it work with signal.lti

    C = sp.eye(1, 4)  

    D = 0

    Ts = 0.01

    G = signal.lti(A, B, C, D)

    H = dscr(G, Ts)

    a, b, c, d = H.A, H.B, H.C, H.D

    rise = 5

    epsilon = 0.1

    N = rise/Ts

    omega = sp.pi/2/N

    r = epsilon**(omega/sp.pi)

    r1 = r

    r2 = 0.9*r

    x1, y1 = pol2cart(omega, r1)

    x2, y2 = pol2cart(omega, r2)

    p1 = x1+y1*1j

    p2 = x2+y2*1j

    p3 = x1-y1*1j

    p4 = x2-y2*1j

    P = [[p1], [p2], [p3], [p4]]

    K = acker(a,b,P) 

    print K