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