# -*- 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.TwoMass()

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

sys.compute_ABCD()


# LQR controller
kp  = 5
ki  = 1
kd  = 2
tau = 0.1

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

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