/***********************************************************
 *
 * @file: pedestrian_sfm_plugin.cpp
 * @breif: Gazebo plugin for pedestrians using social force model
 * @author: Yang Haodong
 * @update: 2023-03-15
 * @version: 1.1
 *
 * Copyright (c) 2023， Yang Haodong
 * All rights reserved.
 * --------------------------------------------------------
 *
 **********************************************************/
#ifndef PEDESTRIANVISUAL_GAZEBO_PLUGIN_H
#define PEDESTRIANVISUAL_GAZEBO_PLUGIN_H

// C++
#include <string>
#include <vector>
#include <algorithm>
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <tf2/utils.h>

// Gazebo
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/util/system.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo_msgs/GetModelState.h>

// message
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <pedsim_msgs/TrackedPersons.h>
#include <pedsim_msgs/TrackedPerson.h>
#include <gazebo_sfm_plugin/ped_state.h>


namespace gazebo
{
  class GZ_PLUGIN_VISIBLE PedestrianVisualPlugin : public ModelPlugin
  {
  public:
    /**
     * @brief Construct a gazebo plugin
     */
    PedestrianVisualPlugin();

    /**
     * @brief De-Construct a gazebo plugin
     */
    ~PedestrianVisualPlugin();

    /**
     * @brief Load the actor plugin.
     * @param _model  Pointer to the parent model.
     * @param _sdf    Pointer to the plugin's SDF elements.
     */
    virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
    
    /**
     * @brief Publish pedestrians visualization information
     */
    void publishPedVisuals();

  private:
    /**
     * @brief Function that is called every update cycle.
     * @param _info Timing information.
     */
    void OnUpdate(const common::UpdateInfo& _info);


  private:
    // Gazebo ROS node
    std::unique_ptr<ros::NodeHandle> node_;
    // topic publisher
    ros::Publisher ped_visual_pub_;
    // Pointer to the parent actor.
    physics::ActorPtr actor_;
    // Pointer to the world, for convenience.
    physics::WorldPtr world_;
    // Pointer to the sdf element.
    sdf::ElementPtr sdf_;
    // List of connections
    std::vector<event::ConnectionPtr> connections_;
    // Update interval
    size_t update_interval_;
    // Update counter
    size_t update_cnt_;

  };
}  // namespace gazebo
#endif
