import numpy as np
import control

# g = 9.8
# L = 1.5
# m = 1.0 # mass of bob (kg)
# M = 5.0  # mass of cart (kg)
# d1 = 1.0
# d2 = 0.5

    #   pendulum_mass: 1.0 (m)
    #   cart_mass: 5.0  (M)
    #   pendulum_length: 2.0
    #   damping_coefficient: 20.0
    #   gravity: -9.8
    #   max_cart_force: 1000.0
    #   noise_level: 0.0

g = 9.8
L = 1.5
m = 1.0
M = 5.0
d1 = 1.0
d2 = 0.5


# Pendulum up (linearized eq)
# Eigen val of A : array([[ 1.        , -0.70710678, -0.07641631,  0.09212131] )
_q = (m+M) * g / (M*L)
A = np.array([\
            [0,1,0,0], \
            [0,-d1, -g*m/M,0],\
            [0,0,0,1.],\
            [0,d1/L,_q,-d2] ] )

B = np.expand_dims( np.array( [0, 1.0/M, 0., -1/(M*L)] ), 1 )



# Pendulum Down - Verified correct.
# Eigen Values of this: array([ 0.00+0.j        , -1.00+0.j        , -0.25+2.78881695j,       -0.25-2.78881695j])
# A = np.array([\
#             [0,1,0,0], \
#             [0,-d1, -g*m/(2*m+M),0],\
#             [0,0,0,1],\
#             [0,0,-(M+m)*g/(M*L),-d2] ] )

#B = np.array( [] )

print ('A\n', A)
print ('B\n', B)

# Controllability
print ('---Controllability\n')
print ('rank of ctrb(A,b) \n' , np.linalg.matrix_rank( control.ctrb( A, B ) ))
print ('Eigenvalues of A \n', np.linalg.eig( A ))


# Pole Placement
K = control.place( A, B, [-1, -2, -4, -5] )
print ('---Pole Placement\nK=\n', K)

# Verification of Eigen values of A-BK
print ('---Verification of Eigen values of A-BK')
print ('Eigenvalues of A-BK \ n', np.linalg.eig( A-np.matmul(B,K) ))
