// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.

#pragma once

#include <string>

#include <gazebo/gazebo.hh>

#include "simulation/gz_msgs/msgs.h"

/**
 * \brief Plugin for reading the angle of a joint.
 *
 * This plugin publishes the angle of a joint to the topic every
 * physics update. Supports reading in either radians or degrees.
 *
 * To add a potentiometer to your robot, add the following XML to your
 * robot model:
 *
 *     <plugin name="my_pot" filename="libpotentiometer.so">
 *       <joint>Joint Name</joint>
 *       <topic>~/my/topic</topic>
 *       <units>{degrees, radians}</units>
 *     </plugin>
 *
 * - `joint`: Name of the joint this potentiometer is attached to.
 * - `topic`: Optional. Message will be published as a gazebo.msgs.Float64.
 *            The default topic name will be the plugin name.
 *            Recommended topic names are of the form:
 *              /gazebo/frc/simulator/analog/n
 *            where n is the analog channel for the pot.
 * - `multiplier`: Optional.  Amount to multiply output with.
 *            Useful when a rotary pot returns something other than an angle
 * - `units`: Optional. Defaults to radians.
 */
class Potentiometer : public gazebo::ModelPlugin {
 public:
  /// \brief Load the potentiometer and configures it according to the sdf.
  void Load(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);

  /// \brief Sends out the potentiometer reading each timestep.
  void Update(const gazebo::common::UpdateInfo& info);

 private:
  /// \brief Publish the angle on this topic.
  std::string topic;

  /// \brief Whether or not this potentiometer measures radians or degrees.
  bool radians;

  /// \brief A scaling factor to apply to this potentiometer.
  double multiplier;

  /// \brief The joint that this potentiometer measures
  gazebo::physics::JointPtr joint;

  /// \brief The model to which this is attached.
  gazebo::physics::ModelPtr model;

  /// \brief Pointer to the world update function.
  gazebo::event::ConnectionPtr updateConn;

  /// \brief The node on which we're advertising.
  gazebo::transport::NodePtr node;

  /// \brief Publisher handle.
  gazebo::transport::PublisherPtr pub;
};