/**
 * \file ipc_base.h
 * \brief definition de structures
 * \author {
 * 			Sébastien Ambroziak
 * 			Paul Sauvage
 * 			}
 * \version 0.1
 * \date 5 novembre 2020
 *
 * Definitions des structures pour la message queue
 *
 */


#ifndef IPC_BASE
#define IPC_BASE

#include <string>
#include <iostream>

#define ROS2 1
#define FLAIR 2
#define ARDUPILOT 3

#include <ctime>

//#define MAX_TOPIC_SIZE 25
//#define MAX_CHAR_ARRAY_SIZE 255

// structures for message queue
namespace ipc{
namespace type{

struct vector2D {
	double x;
	double y;
};

struct vector2D_32 {
	float x;
	float y;
	float z;
};

inline std::ostream &operator<<(std::ostream &out, vector2D vecteur) {
    out << "\tx : " << vecteur.x << std::endl;
	out << "\ty : " << vecteur.y << std::endl;
    return out;
}

struct vector3D {
	double x;
	double y;
	double z;
};

struct vector3D_32 {
	float x;
	float y;
	float z;
};

inline std::ostream &operator<<(std::ostream &out, vector3D vecteur) {
    out << "\tx : " << vecteur.x << std::endl;
	out << "\ty : " << vecteur.y << std::endl;
	out << "\tz : " << vecteur.z << std::endl;
    return out;
}

inline std::ostream &operator<<(std::ostream &out, vector3D_32 vecteur) {
    out << "\tx : " << vecteur.x << std::endl;
	out << "\ty : " << vecteur.y << std::endl;
	out << "\tz : " << vecteur.z << std::endl;
    return out;
}

struct acceleration {
	vector3D linear;
	vector3D angular;
};

struct vector4D {
	double x;
	double y;
	double z;
	double w;
};

struct vector4D_32 {
	float x;
	float y;
	float z;
	float w;
};

inline std::ostream &operator<<(std::ostream &out, vector4D vecteur) {
    out << "\tx : " << vecteur.x << std::endl;
	out << "\ty : " << vecteur.y << std::endl;
	out << "\tz : " << vecteur.z << std::endl;
	out << "\tw : " << vecteur.w << std::endl;
    return out;
}

inline std::ostream &operator<<(std::ostream &out, vector4D_32 vecteur) {
    out << "\tx : " << vecteur.x << std::endl;
	out << "\ty : " << vecteur.y << std::endl;
	out << "\tz : " << vecteur.z << std::endl;
	out << "\tw : " << vecteur.w << std::endl;
    return out;
}

struct matrix {
	int width;
	int height;
	float data[5][5];
};

inline std::ostream &operator<<(std::ostream &out, matrix M) {
    
	for(int j = 0;j < M.height;j++){
		out<<"\t| "<<M.data[0][j];
		for(int i = 1;i < M.width;i++){
			out << "\t " << M.data[i][j];
		}
		out << " |" << std::endl;
	}
    return out;
}

struct imu{
	vector4D orientation;
	double orientation_covariance[9];

	vector3D angular_velocity;
	double angular_velocity_covariance[9];

	vector3D linear_acceleration;
	double linear_acceleration_covariance[9];
};

inline std::ostream &operator<<(std::ostream &out, imu msg) {
	out << "orientation : \n" << msg.orientation << std::endl;
	out << "orientation covarience : " ;
	for (int i=0;i<9;i++){out << msg.orientation_covariance[i] << ' ';} 
	out << std::endl;

	out << "angular_velocity : \n" << msg.angular_velocity << std::endl;
	out << "angular_velocity_covariance : " ;
	for (int i=0;i<9;i++){out << msg.orientation_covariance[i] << ' ';} 
	out << std::endl;

	out << "linear_acceleration : \n" << msg.linear_acceleration << std::endl;
	out << "linear_acceleration_covariance : " ;
	for (int i=0;i<9;i++){out << msg.linear_acceleration_covariance[i] << ' ';} 
	out << std::endl;

    return out;
}

struct ahrs{
	vector4D_32 quaternion;
	vector3D_32 angular_rate;
};

inline std::ostream &operator<<(std::ostream &out, ahrs msg) {
	out << "quaternion : \n" << msg.quaternion << std::endl;
	out << "angular_rate : \n" << msg.angular_rate << std::endl;
    return out;
}


/**
 * \struct ipc_message
 * \brief Structure classique de message ipc.
 * 
 * 	\param T permet de choisir le type de l'élément data
 *  \param N permet de choisir la taille du tableau d'objet du message 
 *  Si N = 0 : data est une structure de type T
 *  Si N > 0 : data est un tableau d'élement de type T et de taille N
 */



struct timestamp{
	uint32_t sec=0;
  	uint32_t nanosec=0;

	void now(){
		struct timespec now;
		timespec_get( &now, TIME_UTC );
		sec = (int32_t)now.tv_sec;
		nanosec = (uint32_t)now.tv_nsec;
	}

	uint64_t toNanosec(){
		return (uint64_t)((uint64_t)sec*1000000000 + (uint64_t)nanosec) ;
	}
};

inline std::ostream &operator<<(std::ostream &out, timestamp stamp) {
    out << "[" << stamp.sec << '.' << stamp.nanosec << ']';
    return out;
}

inline timestamp operator-(timestamp const& a, timestamp const& b)
{
    timestamp resultat;
    resultat.sec = a.sec - b.sec;
	resultat.nanosec = a.nanosec - b.nanosec;
    return resultat;
}

//message ipc

template <typename T>
struct ipc_message {
	long msg_type = 1;
	struct {
		struct timestamp stamp;
		T value;
	} data;

	void time_now(){
		data.stamp.now();
	}
};

template <typename T>
struct ipc_tab {
	long msg_type = 1;
	struct {
		struct timestamp stamp;
		T values[1];
	} data;

	void time_now(){
		data.stamp.now();
	}
};

} // end namespace type
} // end namespace ipc



#endif