#!/usr/bin/python 
# 9.1
import os, sys
sys.path += [os.getcwdu() + os.sep + ".." + os.sep + "python"]

import scipy as sp
from scipy import signal
from myc2d import myc2d
from pp_basic import pp_basic
from desired import desired

# Magnetically suspended ball problem
# Operating conditions

M = 0.05
L = 0.01
R = 1
K = 0.0001
g = 9.81

#Equilibrium conditions
hs = 0.01
i = sqrt(M*g*hs/K)


# State space matrices
a21 = K*i**2/M/hs**2
a23 = - 2*K*i/M/hs
a33 = - R/L
b3 = 1/L
a1 = sp.array([[0, 1, 0], [a21, 0, a23], [0, 0, a33]])
b1 = sp.array([[0], [0], [b3]])
c1 = sp.array([1, 0, 0])
d1 = 0

# Transfer functions
G = signal.lti(a1, b1, c1, d1)
Ts = 0.01
B, A, k = myc2d(G, Ts)

#polynomials are returned
num, den = G.num, G.den

# Transient specifications
rise = 0.15
epsilon = 0.05
phi = desired(Ts, rise, epsilon)

# Controller design
Rc, Sc, Tc, gamm = pp_basic(B, A, k, phi)

# Setting up simulation parameters for basic
st = 0.0001 # desired change in h, in m.
t_init = 0  # simulation start time
t_final = 0.5 # simulation end time

# Setting up simulation parameters for c_ss_cl
N_var = 0
xInitial = [0 0 0]
N = 1
C = 0 
D = 1