from AlexRobotics.control import DPO

###########################
# Objectives
###########################

x_start = np.array([-3.0, 0.0])
x_goal = np.array([0.0, 0.0])

###########################
# Create objects
###########################

Robot = Manipulator.OneLinkManipulator()

PD = linear.PD(kp=5, kd=2)
PID = linear.PID(kp=5, kd=2, ki=4)
CTC = ComputedTorque.ComputedTorqueController(Robot)
SLD = ComputedTorque.SlidingModeController(Robot)
RRT = RandomTree.RRT(Robot, x_start)
VI = DPO.ValueIteration1DOF(Robot, 'quadratic')

############################
# Params
############################

tmax = 8  # max motor torque
Robot.u_ub = np.array([tmax])  # Control Upper Bounds
Robot.u_lb = np.array([-tmax])  # Control Lower Bounds
RRT.x_start = x_start
RRT.discretizeactions(3)
"""

from AlexRobotics.dynamic import Manipulator as M
from AlexRobotics.control import linear as RCL

import matplotlib.pyplot as plt
import numpy as np
""" Define system """

# Define dynamic system
R = M.OneLinkManipulator()

# Define controller
kp = 20
kd = 10
PD_controller = RCL.PD(kp, kd)

# Asign feedback law to the dynamic system
R.ctl = PD_controller.ctl
""" Simulation and plotting """

# Ploting a trajectory
x_start = np.array([-3, 0])
tf = 8
n = int(tf / 0.01) + 1

R.plotAnimation(x_start, tf, n, solver='ode')

# Time plot
#R.Sim.plot_OL()
R.Sim.plot_CL()