#include "algo_builder.h"

const char Identification_String[] = "ID_STRING:Design.xml,On-line";

float Acceleration_g_1_data[3];
float Acceleration_g_2_data[3];
float Add_1_out[1];
float Add_2_out[1];
float Add_3_out[1];
float Arctan2_1_out[1];
float Arctan2_2_out[1];
float Arctan2_3_out[1];
float Buffer_Out_1_out1[1];
float Buffer_Out_2_out1[1];
float Buffer_Out_3_out1[1];
float Buffer_Out_4_out1[1];
float Buffer_Out_5_out1[1];
float Buffer_Out_6_out1[1];
float Buffer_Out_7_out1[1];
float Buffer_Out_8_out1[1];
float Buffer_Out_9_out1[1];
float Constant_Float_1_out[1];
float Constant_Float_2_out[1];
float Constant_Float_3_out[1];
float Constant_Float_4_out[1];
float Constant_Float_5_out[1];
float Constant_Float_6_out[1];
float Constant_Float_7_out[1];
float Demux_Float_1_out1[1];
float Demux_Float_1_out2[1];
float Demux_Float_1_out3[1];
float Demux_Float_2_out3[1];
float Demux_Float_2_out1[1];
float Demux_Float_2_out2[1];
float Demux_Float_3_out3[1];
float Demux_Float_3_out1[1];
float Demux_Float_3_out2[1];
float Humidity_percent_1_data[1];
float Moving_Average_1_out[1];
float Moving_Average_2_out[1];
float Moving_Average_3_out[1];
float Mux_Float_1_out[2];
float Mux_Float_2_out[2];
float Mux_Float_3_out[2];
float Mux_Float_4_out[2];
float Mux_Float_5_out[2];
float Mux_Float_6_out[2];
float Mux_Float_8_out[3];
float Mux_Float_9_out[2];
float Mux_Float_10_out[2];
float Mux_Float_11_out[3];
float Mux_Float_12_out[2];
float Pressure_hPa_1_data[1];
float Quaternions_9X_1_data[4];
float Rad_To_Dec_1_out[1];
float Rad_To_Dec_2_out[1];
float Rad_To_Dec_3_out[1];
float Square_1_out[1];
float Square_2_out[1];
float Square_3_out[1];
float Square_Root_1_out[1];
float Square_Root_2_out[1];
float Square_Root_3_out[1];
float Temperature_C_1_data[1];
float Tilt_Sensing_1_data[3];
int32_t And_1_out[1];
int32_t And_2_out[1];
int32_t And_3_out[1];
int32_t And_4_out[1];
int32_t Greater_than_1_out[1];
int32_t Greater_than_2_out[1];
int32_t Greater_than_3_out[1];
int32_t Greater_than_4_out[1];
int32_t Greater_than_6_out[1];
int32_t Greater_than_8_out[1];
int32_t Less_than_1_out[1];
int32_t Less_than_2_out[1];
int32_t Less_than_3_out[1];
int32_t Less_than_4_out[1];
int32_t Less_than_5_out[1];
int32_t Less_than_9_out[1];
void *Sensor_Hub_1_out;

float Mem1[8];
int32_t Mem2[1];
float Mem3[8];
int32_t Mem4[1];
float Mem5[8];
int32_t Mem6[1];


void Demux_3_float(float *in, float *out1, float *out2, float *out3)
{
	*out1 = in[0];
	*out2 = in[1];
	*out3 = in[2];
}

void Mux_2_float(float *in1, float *in2, float *out)
{
	out[0] = *in1;
	out[1] = *in2;
}

void Mux_3_float(float *in1, float *in2, float *in3, float *out)
{
	out[0] = *in1;
	out[1] = *in2;
	out[2] = *in3;
}

sDISPLAY_INFO display_info_list[] = {
{INFO_TYPE_FUSION,1,4,VAR_TYPE_FLOAT,0,"fusion|Sensor\nFusion|Teapot",0},
{INFO_TYPE_GRAPH,2,1,VAR_TYPE_FLOAT,16,"graph|Humidity||1|19|Humidity||||||1",0},
{INFO_TYPE_GRAPH,3,1,VAR_TYPE_FLOAT,20,"graph|Pressure||1|19|pressure||||||1",0},
{INFO_TYPE_GRAPH,4,1,VAR_TYPE_FLOAT,24,"graph|Temperature||1|19|Temperature||||||1",0},
{INFO_TYPE_ANGLE,5,3,VAR_TYPE_FLOAT,28,"angle|Yaw\nPitch\nRoll|Deg.|Yaw|Pitch|Roll|",0},
{INFO_TYPE_ANGLE,6,3,VAR_TYPE_FLOAT,40,"angle|Theta\nPsi\nPhi|Deg.|Theta|Psi|Phi|",0},
{INFO_TYPE_BAR,7,3,VAR_TYPE_FLOAT,52,"bar|acceleration||1|19|X|Y|Z||||1",0},
{0,0,0,0,0,0,0}};


void AB_Init(void)
{
	Constant_Float_1_out[0] = 0.4;
	Sensor_Hub_Init(0, 100, 1, 1);
	Accelero_Init();
	Constant_Float_2_out[0] = 0.7;
	MotionTL_Init(0);
	Constant_Float_6_out[0] = 0;
	Constant_Float_7_out[0] = 0;
	Accelero_Init();
	MotionFX_Init();
	Constant_Float_4_out[0] = -0.7;
	Constant_Float_3_out[0] = 0;
	Constant_Float_5_out[0] = -0.4;
	Temperature_Init();
	Humidity_Init();
	Pressure_Init();
	Message_Length = 64;
}

void AB_Handler(void)
{
	Sensor_Hub_Handler(&Sensor_Hub_1_out);
	Accelero_Sensor_GetData(Sensor_Hub_1_out, Acceleration_g_1_data);
	Demux_3_float(Acceleration_g_1_data, Demux_Float_1_out1, Demux_Float_1_out2, Demux_Float_1_out3);
	if (Demux_Float_1_out1[0] < Constant_Float_1_out[0]) Less_than_4_out[0] = 1; else Less_than_4_out[0] = 0;
	if (Demux_Float_1_out2[0] < Constant_Float_1_out[0]) Less_than_9_out[0] = 1; else Less_than_9_out[0] = 0;
	if (Demux_Float_1_out3[0] > Constant_Float_2_out[0]) Greater_than_3_out[0] = 1; else Greater_than_3_out[0] = 0;
	if (Greater_than_3_out[0]) {And_4_out[0] = Less_than_4_out[0] && Less_than_9_out[0];}
	if (And_4_out[0]) {TiltSensing_GetData(Sensor_Hub_1_out, Tilt_Sensing_1_data);}
	Demux_3_float(Tilt_Sensing_1_data, Demux_Float_3_out1, Demux_Float_3_out2, Demux_Float_3_out3);
	Mux_2_float(Constant_Float_6_out, Demux_Float_3_out1, Mux_Float_1_out);
	if (And_4_out[0] < 2) Buffer_Out_4_out1[0] = Mux_Float_1_out[And_4_out[0]];
	Mux_2_float(Constant_Float_6_out, Demux_Float_3_out2, Mux_Float_5_out);
	if (And_4_out[0] < 2) Buffer_Out_5_out1[0] = Mux_Float_5_out[And_4_out[0]];
	Mux_2_float(Constant_Float_6_out, Demux_Float_3_out3, Mux_Float_6_out);
	if (And_4_out[0] < 2) Buffer_Out_6_out1[0] = Mux_Float_6_out[And_4_out[0]];
	Mux_3_float(Buffer_Out_4_out1, Buffer_Out_5_out1, Buffer_Out_6_out1, Mux_Float_8_out);
	if (And_4_out[0]) {Accelero_Sensor_GetData(Sensor_Hub_1_out, Acceleration_g_2_data);}
	Demux_3_float(Acceleration_g_2_data, Demux_Float_2_out1, Demux_Float_2_out2, Demux_Float_2_out3);
	MovingAverage(Demux_Float_2_out1, Moving_Average_1_out, 8, Mem1, Mem2);
	Square_1_out[0] = pow(Moving_Average_1_out[0], 2);
	MovingAverage(Demux_Float_2_out2, Moving_Average_2_out, 8, Mem3, Mem4);
	Square_2_out[0] = pow(Moving_Average_2_out[0], 2);
	Add_1_out[0] = Square_1_out[0] + Square_2_out[0];
	Square_Root_2_out[0] = sqrt(Add_1_out[0]);
	Arctan2_3_out[0] = atan2f(Moving_Average_1_out[0], Square_Root_2_out[0]);
	Rad_To_Dec_1_out[0] = (180.0f * Arctan2_3_out[0]) / 3.14159265f;
	Mux_2_float(Constant_Float_7_out, Rad_To_Dec_1_out, Mux_Float_9_out);
	if (And_4_out[0] < 2) Buffer_Out_7_out1[0] = Mux_Float_9_out[And_4_out[0]];
	MovingAverage(Demux_Float_2_out3, Moving_Average_3_out, 8, Mem5, Mem6);
	Square_3_out[0] = pow(Moving_Average_3_out[0], 2);
	Add_3_out[0] = Square_1_out[0] + Square_3_out[0];
	Square_Root_1_out[0] = sqrt(Add_3_out[0]);
	Arctan2_2_out[0] = atan2f(Moving_Average_3_out[0], Square_Root_1_out[0]);
	Rad_To_Dec_3_out[0] = (180.0f * Arctan2_2_out[0]) / 3.14159265f;
	Mux_2_float(Constant_Float_7_out, Rad_To_Dec_3_out, Mux_Float_12_out);
	if (And_4_out[0] < 2) Buffer_Out_9_out1[0] = Mux_Float_12_out[And_4_out[0]];
	Add_2_out[0] = Square_2_out[0] + Square_3_out[0];
	Square_Root_3_out[0] = sqrt(Add_2_out[0]);
	Arctan2_1_out[0] = atan2f(Moving_Average_2_out[0], Square_Root_3_out[0]);
	Rad_To_Dec_2_out[0] = (180.0f * Arctan2_1_out[0]) / 3.14159265f;
	Mux_2_float(Constant_Float_7_out, Rad_To_Dec_2_out, Mux_Float_10_out);
	if (And_4_out[0] < 2) Buffer_Out_8_out1[0] = Mux_Float_10_out[And_4_out[0]];
	Mux_3_float(Buffer_Out_7_out1, Buffer_Out_8_out1, Buffer_Out_9_out1, Mux_Float_11_out);
	Quaternions9X_GetData(Sensor_Hub_1_out, Quaternions_9X_1_data);
	if (Demux_Float_1_out1[0] > Constant_Float_5_out[0]) Greater_than_4_out[0] = 1; else Greater_than_4_out[0] = 0;
	if (Demux_Float_1_out2[0] < Constant_Float_4_out[0]) Less_than_3_out[0] = 1; else Less_than_3_out[0] = 0;
	if (Demux_Float_1_out3[0] > Constant_Float_5_out[0]) Greater_than_2_out[0] = 1; else Greater_than_2_out[0] = 0;
	if (Greater_than_2_out[0]) {And_2_out[0] = Greater_than_4_out[0] && Less_than_3_out[0];}
	if (And_2_out[0]) {Temperature_Sensor_GetData(Sensor_Hub_1_out, Temperature_C_1_data);}
	Mux_2_float(Constant_Float_3_out, Temperature_C_1_data, Mux_Float_2_out);
	if (And_2_out[0] < 2) Buffer_Out_1_out1[0] = Mux_Float_2_out[And_2_out[0]];
	if (Demux_Float_1_out3[0] < Constant_Float_4_out[0]) Less_than_5_out[0] = 1; else Less_than_5_out[0] = 0;
	if (Demux_Float_1_out1[0] > Constant_Float_5_out[0]) Greater_than_8_out[0] = 1; else Greater_than_8_out[0] = 0;
	if (Demux_Float_1_out2[0] > Constant_Float_5_out[0]) Greater_than_6_out[0] = 1; else Greater_than_6_out[0] = 0;
	if (Greater_than_8_out[0]) {And_3_out[0] = Less_than_5_out[0] && Greater_than_6_out[0];}
	if (And_3_out[0]) {Humidity_Sensor_GetData(Sensor_Hub_1_out, Humidity_percent_1_data);}
	Mux_2_float(Constant_Float_3_out, Humidity_percent_1_data, Mux_Float_3_out);
	if (And_3_out[0] < 2) Buffer_Out_3_out1[0] = Mux_Float_3_out[And_3_out[0]];
	if (Demux_Float_1_out3[0] < Constant_Float_1_out[0]) Less_than_1_out[0] = 1; else Less_than_1_out[0] = 0;
	if (Demux_Float_1_out2[0] > Constant_Float_2_out[0]) Greater_than_1_out[0] = 1; else Greater_than_1_out[0] = 0;
	if (Demux_Float_1_out1[0] < Constant_Float_1_out[0]) Less_than_2_out[0] = 1; else Less_than_2_out[0] = 0;
	if (Greater_than_1_out[0]) {And_1_out[0] = Less_than_1_out[0] && Less_than_2_out[0];}
	if (And_1_out[0]) {Pressure_Sensor_GetData(Sensor_Hub_1_out, Pressure_hPa_1_data);}
	Mux_2_float(Constant_Float_3_out, Pressure_hPa_1_data, Mux_Float_4_out);
	if (And_1_out[0] < 2) Buffer_Out_2_out1[0] = Mux_Float_4_out[And_1_out[0]];
	Display_Update(Quaternions_9X_1_data, &display_info_list[0]);
	Display_Update(Buffer_Out_3_out1, &display_info_list[1]);
	Display_Update(Buffer_Out_2_out1, &display_info_list[2]);
	Display_Update(Buffer_Out_1_out1, &display_info_list[3]);
	Display_Update(Mux_Float_8_out, &display_info_list[4]);
	Display_Update(Mux_Float_11_out, &display_info_list[5]);
	Display_Update(Acceleration_g_1_data, &display_info_list[6]);
}