#pragma once

#include "drake/common/drake_copyable.h"
#include "drake/systems/controllers/state_feedback_controller_interface.h"
#include "drake/systems/framework/diagram.h"

namespace drake {
namespace manipulation {
namespace schunk_wsg {

enum class ControlMode { kPosition = 0, kForce = 1 };

// N.B. Inheritance order must remain fixed for pydrake (#9243).
/** This class implements a controller for a Schunk WSG gripper as a
 * `systems::Diagram`. The composition of this diagram is determined by the
 * control mode specified for the controller, which can be either
 * ControlMode::kPosition or ControlMode::kForce. In both cases, the overall
 * layout of the diagram is:
 * ```
 *             ┌─────────────┐
 * joint       │Joint State  │   ┌──────────┐
 * state ─────▶│To Control   ├──▶│          │
 *             │State        │   │          │
 *             └─────────────┘   │PID       │   ╔════════════╗
 *             ╔═════════════╗   │Controller├──▶║            ╟─────┐
 * desired     ║Generate     ║   │          │   ║            ║     │
 * grip ──────▶║Desired      ╟──▶│          │   ║Handle      ║     │
 * state       ║Control State║   └──────────┘   ║Feed-Forward║     │
 *             ╚═════════════╝                  ║Force       ║     │
 * feed                                         ║            ║     │
 * forward ────────────────────────────────────▶║            ╟──┐  │
 * force                                        ╚════════════╝  │  │
 *                                                              │  │
 *                           ┌──────────────────────────────────┘  │
 *                           │              ┌──────────────────────┘
 *                           │              │
 *                           │              │   ┌───────────┐
 *                           │              │   │Mean Finger│   ┌───┐
 *                           │              └──▶│Force To   ├──▶│   │
 *                           │                  │Joint Force│   │   │
 *                           │                  └───────────┘   │   │
 *                           │                                  │ + ├──▶ control
 *                           │   ┌──────────┐   ┌───────────┐   │   │
 *                 ┌─────────│──▶│          │   │Grip Force │   │   │
 *                 │   ┌──┐  └──▶│Saturation├──▶│To Joint   ├──▶│   │
 * max force / 2 ──┴──▶│-1├─────▶│          │   │Force      │   └───┘
 *                     └──┘      └──────────┘   └───────────┘
 * ```
 * The blocks with double outlines (══) differ between the two control modes:
 *
 * - Generate Desired Control State
 *   - ControlMode::kPosition
 * ```
 *        ┌───────────┐
 *        │Desired    │
 *        │Mean Finger├──▶█
 *        │State      │   █   ┌─────────────┐
 *        └───────────┘   █   │Muxed States │    desired
 *                        █──▶│To Control   ├──▶ control
 *                        █   │State        │    state
 *         desired        █   └─────────────┘
 *         grip   ───────▶█
 *         state
 *
 * ```
 *    - ControlMode::kForce
 * ```
 *        ┌───────────┐
 *        │Desired    │                          desired
 *        │Mean Finger├────────────────────────▶ control
 *        │State      │                          state
 *        └───────────┘
 *
 *         desired        ┌────────┐
 *         grip   ───────▶│IGNORED │
 *         state          └────────┘
 * ```
 * - Handle Feed-Forward Force
 *   - ControlMode::kPosition
 * ```
 *                                     █────▶ mean finger force
 *         pid                         █
 *         controller ────────────────▶█
 *         output                      █
 *                                     █────▶ grip force
 *         feed           ┌────────┐
 *         forward ──────▶│IGNORED │
 *         force          └────────┘
 * ```
 *   - ControlMode::kForce
 * ```
 *         pid
 *         controller ──────────────────────▶ mean finger force
 *         output
 *
 *         feed
 *         forward ─────────────────────────▶ grip force
 *         force
 *
 * ```
 * The remaining blocks differ only in their numerical parameters.
 *
 * Note that the "feed forward force" input is ignored for
 * ControlMode::kPosition and the "desired grip state" input is ignored for
 * ControlMode::kPosition.
 *
 * @system
 * name: SchunkWsgPlainController
 * input_ports:
 * - joint_state
 * - max_force
 * - <span style="color:gray">desired_grip_state</span>
 * - <span style="color:gray">feed_forward_force</span>
 * output_ports:
 * - control
 * @endsystem
 *
 * The `desired_grip_state` port is present only when the control mode is
 * `kPosition`; the `feed_forward_force` port is present only when the control
 * mode is `kForce`.
 *
 * @ingroup manipulation_systems
 */
class SchunkWsgPlainController
    : public systems::Diagram<double>,
      public systems::controllers::StateFeedbackControllerInterface<double> {
 public:
  DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(SchunkWsgPlainController);
  /** Specify control gains and mode. Mode defaults to position control. */
  explicit SchunkWsgPlainController(
      ControlMode control_mode = ControlMode::kPosition, double kp = 2000,
      double ki = 0, double kd = 5);

  /** Returns the feed-forward force input port.
   * @pre `this` was constructed with `control_mode` set to
   * `ControlMode::kForce`.*/
  const systems::InputPort<double>& get_input_port_feed_forward_force() const {
    DRAKE_ASSERT(feed_forward_force_input_port_ >= 0);
    return this->get_input_port(feed_forward_force_input_port_);
  }

  const systems::InputPort<double>& get_input_port_max_force() const {
    return this->get_input_port(max_force_input_port_);
  }

  // These methods implement StateFeedbackControllerInterface.
  const systems::InputPort<double>& get_input_port_estimated_state()
      const override {
    return this->get_input_port(state_input_port_);
  }

  /** Returns the desired grip state input port.
   * @pre `this` was constructed with `control_mode` set to
   * `ControlMode::kPosition`.*/
  const systems::InputPort<double>& get_input_port_desired_state()
      const override {
    DRAKE_ASSERT(state_input_port_ >= 0);
    return this->get_input_port(desired_grip_state_input_port_);
  }

  const systems::OutputPort<double>& get_output_port_control() const override {
    return systems::Diagram<double>::get_output_port(0);
  }

 private:
  systems::InputPortIndex desired_grip_state_input_port_;
  systems::InputPortIndex feed_forward_force_input_port_;
  systems::InputPortIndex state_input_port_;
  systems::InputPortIndex max_force_input_port_;
};

}  // namespace schunk_wsg
}  // namespace manipulation
}  // namespace drake
