/home/usappz/DeadalusSim/source/gazebo_plugins/gazebo_model_plugin.hh

/* -----------------------------------------------------------------------------
 * 
 *
 *  ____                   _____     _      
 * / ___|  __ _ _ __  _ __|__  /  __| | ___ 
 * \___ \ / _` | '_ \| '_ \ / /  / _` |/ _ \
 *  ___) | (_| | |_) | |_) / /_ | (_| |  __/
 * |____/ \__,_| .__/| .__/____(_)__,_|\___|
 *             |_|   |_| 
 * 
 *
 * 
 * Author: Andreas Gschossmann
 * Email:  andreas.gschossmann@hs-regensburg.de
 * 
 * gazebo_model_plugin.cc - gazebo-plugin to get access to gazebo-API
 *
 * ----------------------------------------------------------------------------*/

#ifndef _GAZEBO_MODEL_PLUGIN_HH_ 
#define _GAZEBO_MODEL_PLUGIN_HH_

#include <iostream>
#include <math.h>
#include <boost/shared_ptr.hpp>


#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>

#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>

#include <boost/shared_ptr.hpp>
#include <sdf/sdf.hh>
#include <boost/gil/gil_all.hpp>
#include <boost/gil/extension/io/png_dynamic_io.hpp>

//#include "gazebo/math/Vector3.hh"
//#include "gazebo/physics/physics.hh"
//#include "imu.pb.h"

#include "../quadrotor_dynamics/quadrotor_dynamics.hh"
#include "../dependencies/resources.hh"

#include "../sil/sil.hh"

namespace gazebo
{  


  class GazeboModelPlugin : public ModelPlugin
  {
    //private
      transport::NodePtr node;                  
      transport::SubscriberPtr imuSubscriber;   

    public: GazeboModelPlugin();  //: ModelPlugin();//, _myQuadRotorDynamics(), _rotor_speed()
    //public: virtual ~GazeboModelPlugin();

    public:
      QuadRotorDynamics _myQuadRotorDynamics;
      AngularVel        _rotor_speed;           
      Mechanics         _mechanics;             


    private:
      SIL               _sil;                   
      Sensors           _sensor_vals;           


    private:
      enum MotorNumber {m1, m2, m3, m4, front, rear, right, left, all};

    public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/);

    public: void callback_imu(ConstIMUPtr &msg);

    public: void OnUpdate(const common::UpdateInfo & /*_info*/);

    private: physics::ModelPtr model;
    // Pointer to the motor links 
    private: 
      physics::LinkPtr motor_1, motor_front;
      physics::LinkPtr motor_2, motor_right; 
      physics::LinkPtr motor_3, motor_rear;
      physics::LinkPtr motor_4, motor_left;

    // Pointer to the center Link
    private:
      physics::LinkPtr frame_center;

    // Pointer to the motor rod links
    private:
      physics::LinkPtr motor_prop_1, motor_prop_front;
      physics::LinkPtr motor_prop_2, motor_prop_right;
      physics::LinkPtr motor_prop_3, motor_prop_rear;
      physics::LinkPtr motor_prop_4, motor_prop_left;

    // Pointer to the update event connection
    private: event::ConnectionPtr updateConnection;

    // Gazebo Time
    private: 
      common::Time _current_time;
      common::Time _last_time;
      common::Time _dt;

    private:
      const gazebo::math::Vector3 _earth_mag_vec3;

    private: void get_magnetic_field(); 
    // Set motor-values: forces, angular velocities, moments (own approach)
    //public: bool SetPhysics(const MotorNumber myMotorNmb, double value, bool update = false);
    //public: bool SetThrust(const MotorNumber myMotorNmb, double value);

  };
}

#endif