#!/usr/bin/python 
# 9.9
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_im import pp_im
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
a = sp.array([[0, 1, 0], [a21, 0, a23], [0, 0, a33]])
b = sp.array([[0], [0], [b3]])
c = sp.array([1, 0, 0])
d = 0

# Transfer functions
G = signal.lti(a, b, c, d)
Ts = 0.01
B, A, k = myc2d(G,Ts)

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

# Controller design
Delta = sp.array([1, -1])
Rc, Sc, Tc, gamm = pp_im(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