load(
    "@drake//tools/skylark:drake_cc.bzl",
    "drake_cc_googletest",
    "drake_cc_library",
    "drake_cc_package_library",
)
load("//tools/lint:lint.bzl", "add_lint_tests")

package(default_visibility = ["//visibility:public"])

drake_cc_package_library(
    name = "controllers",
    visibility = ["//visibility:public"],
    deps = [
        ":dynamic_programming",
        ":finite_horizon_linear_quadratic_regulator",
        ":inverse_dynamics",
        ":inverse_dynamics_controller",
        ":joint_stiffness_controller",
        ":linear_model_predictive_controller",
        ":linear_quadratic_regulator",
        ":pid_controlled_system",
        ":pid_controller",
        ":state_feedback_controller_interface",
        ":zmp_planner",
    ],
)

drake_cc_library(
    name = "state_feedback_controller_interface",
    hdrs = ["state_feedback_controller_interface.h"],
    deps = [
        "//systems/framework:system",
    ],
)

drake_cc_library(
    name = "dynamic_programming",
    srcs = ["dynamic_programming.cc"],
    hdrs = ["dynamic_programming.h"],
    deps = [
        "//common:essential",
        "//math:wrap_to",
        "//solvers:mathematical_program",
        "//solvers:solve",
        "//systems/analysis:simulator",
        "//systems/framework",
        "//systems/primitives:barycentric_system",
    ],
)

drake_cc_library(
    name = "finite_horizon_linear_quadratic_regulator",
    srcs = ["finite_horizon_linear_quadratic_regulator.cc"],
    hdrs = ["finite_horizon_linear_quadratic_regulator.h"],
    deps = [
        "//common/trajectories",
        "//math:autodiff",
        "//math:gradient",
        "//math:matrix_util",
        "//systems/analysis:simulator",
        "//systems/framework",
    ],
)

drake_cc_library(
    name = "inverse_dynamics",
    srcs = ["inverse_dynamics.cc"],
    hdrs = ["inverse_dynamics.h"],
    deps = [
        "//multibody/plant",
        "//systems/framework",
    ],
)

drake_cc_library(
    name = "inverse_dynamics_controller",
    srcs = ["inverse_dynamics_controller.cc"],
    hdrs = ["inverse_dynamics_controller.h"],
    deps = [
        ":inverse_dynamics",
        ":pid_controller",
        ":state_feedback_controller_interface",
        "//multibody/plant",
        "//systems/framework",
        "//systems/primitives:adder",
        "//systems/primitives:constant_vector_source",
        "//systems/primitives:demultiplexer",
        "//systems/primitives:pass_through",
    ],
)

drake_cc_library(
    name = "joint_stiffness_controller",
    srcs = ["joint_stiffness_controller.cc"],
    hdrs = ["joint_stiffness_controller.h"],
    deps = [
        "//multibody/plant",
        "//systems/framework",
    ],
)

drake_cc_library(
    name = "linear_model_predictive_controller",
    srcs = ["linear_model_predictive_controller.cc"],
    hdrs = ["linear_model_predictive_controller.h"],
    deps = [
        "//common/trajectories:piecewise_polynomial",
        "//planning/trajectory_optimization:direct_transcription",
        "//solvers:solve",
        "//systems/primitives:linear_system",
    ],
)

drake_cc_library(
    name = "linear_quadratic_regulator",
    srcs = ["linear_quadratic_regulator.cc"],
    hdrs = ["linear_quadratic_regulator.h"],
    deps = [
        "//common:is_approx_equal_abstol",
        "//math:continuous_algebraic_riccati_equation",
        "//math:discrete_algebraic_riccati_equation",
        "//systems/framework",
        "//systems/primitives:linear_system",
    ],
)

drake_cc_library(
    name = "pid_controller",
    srcs = ["pid_controller.cc"],
    hdrs = ["pid_controller.h"],
    deps = [
        ":state_feedback_controller_interface",
        "//systems/framework:leaf_system",
        "//systems/primitives:matrix_gain",
    ],
)

drake_cc_library(
    name = "pid_controlled_system",
    srcs = ["pid_controlled_system.cc"],
    hdrs = ["pid_controlled_system.h"],
    deps = [
        ":pid_controller",
        "//systems/primitives:adder",
        "//systems/primitives:constant_vector_source",
        "//systems/primitives:saturation",
    ],
)

drake_cc_library(
    name = "zmp_planner",
    srcs = ["zmp_planner.cc"],
    hdrs = ["zmp_planner.h"],
    deps = [
        ":linear_quadratic_regulator",
        "//common:essential",
        "//common/trajectories:piecewise_polynomial",
    ],
)

# === test/ ===

drake_cc_library(
    name = "zmp_test_util",
    testonly = 1,
    srcs = ["test/zmp_test_util.cc"],
    hdrs = ["test/zmp_test_util.h"],
    deps = [
        ":zmp_planner",
    ],
)

drake_cc_googletest(
    name = "dynamic_programming_test",
    # Test timeout increased to not timeout when run with Valgrind.
    timeout = "long",
    data = [
        "//examples/pendulum:models",
    ],
    deps = [
        ":dynamic_programming",
        ":linear_quadratic_regulator",
        "//common/proto:call_python",
        "//common/test_utilities:eigen_matrix_compare",
        "//multibody/parsing",
        "//multibody/plant",
        "//systems/framework:diagram_builder",
        "//systems/primitives:integrator",
        "//systems/primitives:linear_system",
    ],
)

drake_cc_googletest(
    name = "finite_horizon_linear_quadratic_regulator_test",
    deps = [
        ":finite_horizon_linear_quadratic_regulator",
        ":linear_quadratic_regulator",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_no_throw",
        "//systems/framework/test_utilities:scalar_conversion",
        "//systems/primitives:linear_system",
        "//systems/primitives:symbolic_vector_system",
    ],
)

drake_cc_googletest(
    name = "inverse_dynamics_test",
    data = [
        "//manipulation/models/iiwa_description:models",
    ],
    deps = [
        ":inverse_dynamics",
        "//common:find_resource",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//multibody/parsing",
        "//systems/controllers/test_utilities",
    ],
)

drake_cc_googletest(
    name = "inverse_dynamics_controller_test",
    data = [
        "//manipulation/models/iiwa_description:models",
    ],
    deps = [
        ":inverse_dynamics_controller",
        "//common:find_resource",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//multibody/parsing",
        "//systems/controllers/test_utilities",
    ],
)

drake_cc_googletest(
    name = "joint_stiffness_controller_test",
    data = [
        "//manipulation/models/iiwa_description:models",
        "//multibody/benchmarks/acrobot:models",
    ],
    deps = [
        ":joint_stiffness_controller",
        "//common:find_resource",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//multibody/parsing",
    ],
)

drake_cc_googletest(
    name = "linear_model_predictive_controller_test",
    deps = [
        ":linear_model_predictive_controller",
        "//common/test_utilities:eigen_matrix_compare",
        "//math:discrete_algebraic_riccati_equation",
        "//systems/analysis:simulator",
    ],
)

drake_cc_googletest(
    name = "linear_quadratic_regulator_test",
    data = ["//examples/acrobot:models"],
    deps = [
        ":linear_quadratic_regulator",
        "//common:find_resource",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_no_throw",
        "//examples/acrobot:acrobot_plant",
        "//multibody/parsing",
        "//multibody/plant",
    ],
)

drake_cc_googletest(
    name = "pid_controlled_system_test",
    deps = [
        ":pid_controlled_system",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_no_throw",
    ],
)

drake_cc_googletest(
    name = "pid_controller_test",
    deps = [
        ":pid_controller",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_no_throw",
    ],
)

drake_cc_googletest(
    name = "zmp_planner_test",
    deps = [
        ":zmp_test_util",
        "//common/test_utilities:eigen_matrix_compare",
    ],
)

add_lint_tests()