/home/usappz/DeadalusSim/source/dependencies/resources.hh

#ifndef _RESOURCES_HH_ 
#define _RESOURCES_HH_

#include <string>

//#include <math.h>

#include <gazebo/gazebo.hh>

#include <gazebo/physics/physics.hh>
#include <gazebo/msgs/msgs.hh>
#include <gazebo/math/Vector3.hh>

#include <stdio.h>
#include "constants.hh" 

using namespace gazebo;

class AngularVel 
{
  public: AngularVel() : 
    motor()
  {};
    
  public: 
    double motor[4];
  
  public: AngularVel get_acceleration( AngularVel angular_vel_current, AngularVel angular_vel_last, double dT )
  {
    AngularVel alpha;
    for (int i=0; i<4; i++)
    {
      // alpha = (Omega_current - Omega_last) / dt     
      alpha.motor[i] = ( angular_vel_current.motor[i] - angular_vel_last.motor[i] ) / dT;
    }
    return alpha;
  }
};

class ActuationVal 
{
  public: ActuationVal() :
    motor()
  {};
  
  public:
    int motor[4]; // never negative
};

class Thrust
{
  public: Thrust() :
    motor()
  {};

  public:
    double motor[4];

  private:   
    gazebo::math::Vector3 motor_vec3[4];        

 public: 
    gazebo::math::Vector3 vec3(int nmb_motor)
    {
      motor_vec3[nmb_motor].Set (0.0, 0.0, motor[nmb_motor]);
      return motor_vec3[nmb_motor];
    } 

 public:
    gazebo::math::Vector3 all_vec3()
    {
      gazebo::math::Vector3 myThrust (0.0, 0.0, motor[0] + motor[1] + motor[2] + motor[3]);
      return myThrust;
    }
       
};

class Torque //in Torque umbenennen
{
  public:
    double roll;// = 0.0;
    double pitch;// = 0.0;
    double yaw;// = 0.0;  

  private:
    gazebo::math::Vector3 torque_vec3;

 public:
    gazebo::math::Vector3 vec3()
    {
      torque_vec3.Set (roll, pitch, yaw);
      return torque_vec3;
    };

  public: void set_torque(double r, double p, double y)
  {
    roll = r;
    pitch = p;
    yaw = y;
  };
};

class Mechanics
{ 
  public: 
    Thrust  thrust; 
    Torque  torque; 
};

class GPS
{
  // GPS 
  public:
    double altitude;
    double latitude_deg;
    double longitude_deg;
};

class IMU 
{
  public:
    double acc_x, acc_y, acc_z;
    double gyro_x, gyro_y, gyro_z;
    double roll_actual, pitch_actual, yaw_actual;
  
  private: 
    gazebo::msgs::Vector3d acc_vec3;
    gazebo::msgs::Vector3d gyro_vec3;

    /*public: gazebo::msgs::Vector3d get_acc_vec3()
    {
      acc_vec3.Set (acc_x, acc_y, acc_z);
      return acc_vec3;
    };

    public: gazebo::msgs::Vector3d get_gyro_vec3()
    {
      gyro_vec3.Set (gyro_x, gyro_y, gyro_z);
      return gyro_vec3;
    };*/

  public: void set_acc(double a_x, double a_y, double a_z)
  {
    acc_x = a_x;
    acc_y = a_y;
    acc_z = a_z;
  };

  public: void set_acc(gazebo::msgs::Vector3d acc)
  {
    acc_vec3 = acc;
    acc_x = acc_vec3.x();
    acc_y = acc_vec3.y();
    acc_z = acc_vec3.z();
  };

  public: void set_gyro(double g_x, double g_y, double g_z)
  {
    gyro_x = g_x;
    gyro_y = g_y;
    gyro_z = g_z;
  };

  public: void set_gyro(gazebo::msgs::Vector3d gyro)
  {
    gyro_vec3 = gyro;
    gyro_x = gyro_vec3.x();
    gyro_y = gyro_vec3.y();
    gyro_z = gyro_vec3.z();
  };
  
  public: void set(gazebo::msgs::Vector3d acc, gazebo::msgs::Vector3d gyro)
  {
    set_gyro(acc);
    set_acc(gyro);
  };

};
 
class MAG
{
  public:
    double mag_x, mag_y, mag_z;

  private:
    gazebo::math::Vector3 mag_vec3;
    
    /*public: gazebo::math::Vector3 get_mag_vec3()
    {
      mag_vec3.Set (mag_x, mag_y, mag_z);
      return mag_vec3;
    };*/

  public: void set(double m_x, double m_y, double m_z)
  {
    mag_x = m_x;
    mag_y = m_y;
    mag_z = m_z;
  };
 
  public: void set(gazebo::math::Vector3 mag)
  {
    mag_vec3 = mag;
    mag_x = mag_vec3.x;
    mag_y = mag_vec3.y;
    mag_z = mag_vec3.z;
  };

  //public: gazebo::msgs::Vector3d

};

class Sensors
{
  public:
    IMU         imu;
    MAG         mag;
    GPS         gps;
};


#endif