//
// calc.c
//

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

Data9DofDescr AddedValues = {0,0,0,0,0,0,0,0,0}; // zwingend erforderlich mit 0 zu initiieren
Data9DofDescr Mittelwert;
Data9DofDescr real;
Matrix3x3 RCM;
Matrix3x3 D_Matrix;

/// zusammenfuehren von LSB und MSB
/**
  \param MSB most significant byte der gelesenen Daten
  \param LSB least significant byte der gelesenen Daten

	Hier wurde sogenanntes Boxing verwendet um auch negative Messwerte zu beruecksichtigen
*/
float sampling (BYTE MSB, BYTE LSB)
{
	unsigned short Val = (unsigned short)((MSB << 8) | LSB);	
	if (Val>(0x7fff)) 
		return (float) ((0xffff-Val)*(-1));
	else
		return (float) Val;
}

/// Mittelwertbildung und Umrechnung in physikalisch Daten 
/**
  \param Avg bestimmt wieviele Datensätze gemittelt werden sollen

Zunaechst wird der Mittelwert ueber die zuvor erfassten und addierten Daten gebildet
Anschliessend werden die Rohdaten in physikalische Messdaten umgewandelt und auf bestimmte Werte normiert
*/
bool AvgData (float Avg)
{
	if (cnt_rec_Data () == Avg)
	{
		Reset_rec_Data();
	
		Mittelwert.AccX = ((float) AddedValues.AccX / Avg);		// casten auf float, um sicherzustellen, dass der Mittelwert vom typ float ist !!
		Mittelwert.AccY = ((float) AddedValues.AccY / Avg);
		Mittelwert.AccZ = ((float) AddedValues.AccZ / Avg);
		Mittelwert.GyroX = ((float) AddedValues.GyroX / Avg);
		Mittelwert.GyroY = ((float) AddedValues.GyroY / Avg);
		Mittelwert.GyroZ = ((float) AddedValues.GyroZ / Avg);
		Mittelwert.MagX = ((float) AddedValues.MagX / Avg);
		Mittelwert.MagY = ((float) AddedValues.MagY / Avg);
		Mittelwert.MagZ = ((float) AddedValues.MagZ / Avg);
		
		AddedValues.AccX =0;AddedValues.AccY =0;AddedValues.AccZ =0;
		AddedValues.MagX =0;AddedValues.MagY =0;AddedValues.MagZ =0;
		AddedValues.GyroX =0;AddedValues.GyroY =0;AddedValues.GyroZ =0;
		
		// convert to physical sizes
		real.AccX = (float) (Mittelwert.AccX / (gravitation * Norm_Acc));
		real.AccY = (float) (Mittelwert.AccY / (gravitation * Norm_Acc));
		real.AccZ = (float) (Mittelwert.AccZ / (gravitation * Norm_Acc));
		
		real.GyroX = (float) (Mittelwert.GyroX * (pi/180U) / Norm_Gyro);
		real.GyroY = (float) (Mittelwert.GyroY * (pi/180U) / Norm_Gyro);
		real.GyroZ = (float) (Mittelwert.GyroZ * (pi/180U) / Norm_Gyro);
		
		real.MagX = (float) (Mittelwert.MagX / Norm_Mag);
		real.MagY = (float) (Mittelwert.MagY / Norm_Mag);
		real.MagZ = (float) (Mittelwert.MagZ / Norm_Mag);

		return 1;
	}
	else
		return 0;
}

/// Addition der gelesenen Werte
/**
  \param actual aktuelle Daten die vom Sensor gelesen wurden
*/
BYTE add (Data9DofDescr actual)
{
	AddedValues.AccX += actual.AccX;
	AddedValues.AccY += actual.AccY;
	AddedValues.AccZ += actual.AccZ;
	AddedValues.GyroX += actual.GyroX;
	AddedValues.GyroY += actual.GyroY;
	AddedValues.GyroZ += actual.GyroZ;
	AddedValues.MagX += actual.MagX;
	AddedValues.MagY += actual.MagY;
	AddedValues.MagZ += actual.MagZ;
	
	return 0;
}

/// allgemeine Multiplikation einer Matrix mit einem Vektor
/**
  \param matrix Werte der uebergebenen Matrix
  \param vec Werte des uebergebenen Vektors
	\param erg Ergebnisvektor
*/
VektorR3 mat_x_vec (Matrix3x3 matrix, VektorR3 vec)
{
	VektorR3 erg;
	erg.X = (matrix.A11 * vec.X) + (matrix.A12 * vec.Y) + (matrix.A13 * vec.Z);
	erg.Y = (matrix.A21 * vec.X) + (matrix.A22 * vec.Y) + (matrix.A23 * vec.Z);
	erg.Z = (matrix.A31 * vec.X) + (matrix.A32 * vec.Y) + (matrix.A33 * vec.Z);
	
	return erg;
}

/// Berechnung der Winkel
/**
	\param ACC Betrag des Beschleunigungsvektors
	\param calced berechnete Winkel
*/
angles calc_angle (void)
{
	static double ACC;		// Betrag des Vektors
	static angles calced;
	
ACC = sqrt( pow((double)real.AccX, 2) + pow((double)real.AccY, 2) + pow((double)real.AccZ, 2) );
		calced.Roll = atan2 ((double) -real.AccZ, (double) -real.AccY);
		calced.Nick = asin ( (double) (-real.AccX/ACC) );
		calced.Gier = atan2 (real.MagY, real.MagX);
	
	return calced;
}

/// Berechnung des Gyro Offsets
VektorR3 calc_Gyro_Offset (void)
{
	static VektorR3 gyroOffset;
	gyroOffset.X = real.GyroX;
	gyroOffset.Y = real.GyroY;
	gyroOffset.Z = real.GyroZ;
	return gyroOffset;
}


/// Berechnung der Winkel
/**
  \param gyroOffset Offset des Sensor der in der Funktion calc_Gyro_Offset gebildet wurde
	\param DCalced Zeitliche Ableitung der Eulerwinkel bestimmt
	\param analysedGyro aktuelle Winkel die durch das Gyroskop bestimmt wurden
	\param analysedGyroAngles dient zur Konvertierung
	\param AccAct aktuelle Winkel bestimmt durch den Beschleunigungssensor
	\param final finale Winkel 

Die finalen Winkel werden berechnet 
durch eine Wichtung der berechneten Winkel des Gyroskops und der berechneten Winkel des Beschleunigungssensors
*/
angles analyse (VektorR3 gyroOffset)
{
	VektorR3 gyroVec;
	Matrix3x3 DCalced;
	VektorR3 analysedGyro;
	angles analysedGyroAngles;
	angles AccAct;
	angles final;
	
	gyroVec.X = real.GyroX - gyroOffset.X;
	gyroVec.Y = real.GyroY -	gyroOffset.Y;
	gyroVec.Z = real.GyroZ - gyroOffset.Z;
	
	AccAct = calc_angle ();
	
	DCalced = calc_D_Matrix (AccAct);
	
	analysedGyro = mat_x_vec (DCalced, gyroVec);
	analysedGyroAngles.Roll = (double) analysedGyro.X;
	analysedGyroAngles.Nick = (double) analysedGyro.Y;
	analysedGyroAngles.Gier = (double) analysedGyro.Z;
	
	// Finale Winkel
	final.Roll = ((analysedGyroAngles.Roll + AccAct.Roll) * Wichtung + AccAct.Roll * (1-Wichtung))*180/pi;
	final.Nick = ((analysedGyroAngles.Nick + AccAct.Nick) * Wichtung + AccAct.Nick * (1-Wichtung))*180/pi;
	final.Gier = ((analysedGyroAngles.Gier + AccAct.Gier) * Wichtung + AccAct.Gier * (1-Wichtung))*180/pi;
	
	return final;
}


/// Berechnung der D Matrix
/**
  \param act uebergabe der aktuellen Winkel

Siehe Handout S.63
*/
Matrix3x3 calc_D_Matrix (angles act)
{
	D_Matrix.A11 = 1;
	D_Matrix.A12 = sin (act.Roll) * tan (act.Nick);
	D_Matrix.A13 = cos (act.Roll) * tan (act.Nick);
	D_Matrix.A21 = 0;
	D_Matrix.A22 = cos (act.Roll);
	D_Matrix.A23 = -sin (act.Roll);
	D_Matrix.A31 = 0;
	D_Matrix.A32 = sin (act.Roll) / cos (act.Nick);
	D_Matrix.A33 = cos (act.Roll) / cos (act.Nick);
	
	return D_Matrix;
}