#ifndef __CALC_H__
#define __CALC_H__

#include <RB_RCC.h>
#include <Timer.h>

#define gravitation (float) 9.81
#define pi (float) 3.1415
#define Norm_Acc (float) 27
#define Norm_Gyro (float) 14.375
#define Norm_Mag (float) 1090
#define Wichtung (float) 0.97

/// Struktur für die 3 verschiedene Sensoren Acc/Gyro/Mag in XYZ-Richtung
typedef struct
{
	float AccX;
	float AccY;
	float AccZ;
	float GyroX;
	float GyroY;
	float GyroZ;
	float MagX;
	float MagY;
	float MagZ;
} Data9DofDescr;

/// Struktur für die Werte einer 3x3 Matrix
typedef struct
{
	double A11;
	double A12;
	double A13;
	double A21;
	double A22;
	double A23;
	double A31;
	double A32;
	double A33;
} Matrix3x3;

/// Struktur für die Werte eines Vektors mit XYZ
typedef struct 
{
	float X;
	float Y;
	float Z;
} VektorR3;

/// Struktur für die Roll-,Nick und Gierwinkel
typedef struct
{
	double Roll;
	double Nick;
	double Gier;
} angles;


/// Prototypen
float sampling (BYTE MSB, BYTE LSB);

bool AvgData (float Avg);

VektorR3 mat_x_vec (Matrix3x3 matrix, VektorR3 vec);

angles analyse (VektorR3 gyroOffset);

angles calc_angle (void);

VektorR3 calc_Gyro_Offset (void);

Matrix3x3 calc_D_Matrix (angles act);

BYTE add (Data9DofDescr actual);

//Matrix3x3 calc_RCM (angles actual);

#endif