# -*- coding: utf-8 -*-
"""
Created on Jun 2 2021

@author: Alex
"""
import numpy as np

from pyro.dynamic               import massspringdamper
from pyro.control.linear        import PIDController


# Plant
sys = massspringdamper.ThreeMass()

sys.m1 = 2
sys.m2 = 3
sys.k1 = 0.01
sys.k2 = 2
sys.b1 = 0.1
sys.b2 = 0.2

sys.compute_ABCD()

sys.C = np.array([[0, 0, 1.0, 0, 0, 0]])

sys.plot_linearized_bode()

# PID controller
kp  = 10
ki  = 1
kd  = 1

tau = 0.05

ctl = PIDController(kp, ki, kd, tau)

ctl.rbar[0] = 2 # reference

# Simulation Closed-Loop Non-linear with LQR controller
cl_sys = ctl + sys
cl_sys.x0[2] = 0
cl_sys.compute_trajectory( 300 )
cl_sys.plot_trajectory('xu')
cl_sys.plot_linearized_pz_map()
cl_sys.plot_linearized_bode()
cl_sys.animate_simulation()