#include "algo_builder.h"

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

float Acceleration_g_1_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 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 Constant_Float_8_out[1];
float Constant_Float_9_out[1];
float Constant_Float_10_out[1];
float Constant_Float_11_out[1];
float Constant_Float_12_out[1];
float Constant_Float_13_out[1];
float Demux_Float_1_out1[1];
float Demux_Float_1_out2[1];
float Demux_Float_1_out3[1];
float Demux_Float_2_out2[1];
float Demux_Float_2_out1[1];
float Demux_Float_2_out3[1];
float Humidity_percent_1_data[1];
float Moving_Average_1_out[1];
float Moving_Average_2_out[1];
float Moving_Average_4_out[1];
float Mux_Float_1_out[3];
float Mux_Float_2_out[3];
float Pressure_hPa_1_data[1];
float Rad2Dec_1_out[1];
float Rad2Dec_2_out[1];
float Rad2Dec_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 Switch_Float_1_out[1];
float Switch_Float_2_out[1];
float Switch_Float_3_out[1];
float Switch_Float_4_out[1];
float Switch_Float_5_out[1];
float Switch_Float_6_out[1];
float Temperature_C_1_data[1];
int32_t And_1_out[1];
int32_t And_2_out[1];
int32_t And_3_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 Less_than_1_out[1];
int32_t Less_than_2_out[1];
int32_t Less_than_3_out[1];
void *Sensor_Hub_1_out;

float Mem1[50];
int32_t Mem2[1];
float Mem3[50];
int32_t Mem4[1];
float Mem5[50];
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_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_ANGLE,1,3,VAR_TYPE_FLOAT,0,"angle|Theta\nPsi\nPhi|Deg.|Theta|Psi|Phi|",0},
{INFO_TYPE_ANGLE,2,1,VAR_TYPE_FLOAT,12,"angle|Humidity|percent|Humidity|||",0},
{INFO_TYPE_ANGLE,3,1,VAR_TYPE_FLOAT,16,"angle|Temperature|Deg|Temperature|||",0},
{INFO_TYPE_ANGLE,4,1,VAR_TYPE_FLOAT,20,"angle|Pressure|hPa|Pressure|||",0},
{0,0,0,0,0,0,0}};


void AB_Init(void)
{
	Sensor_Hub_Init(0, 100, 0, 0);
	Accelero_Init();
	Constant_Float_7_out[0] = 0;
	Constant_Float_2_out[0] = 45;
	Constant_Float_1_out[0] = 45;
	Constant_Float_9_out[0] = 0;
	Constant_Float_10_out[0] = 0;
	Constant_Float_11_out[0] = 0;
	Temperature_Init();
	Constant_Float_3_out[0] = -45;
	Constant_Float_4_out[0] = 45;
	Constant_Float_12_out[0] = 0;
	Humidity_Init();
	Constant_Float_8_out[0] = 145;
	Constant_Float_13_out[0] = 0;
	Pressure_Init();
	Constant_Float_5_out[0] = 45;
	Constant_Float_6_out[0] = 45;
	Message_Length = 24;
}

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);
	MovingAverage(Demux_Float_1_out2, Moving_Average_4_out, 50, Mem1, Mem2);
	MovingAverage(Demux_Float_1_out1, Moving_Average_2_out, 50, Mem3, Mem4);
	Square_3_out[0] = pow(Moving_Average_2_out[0], 2);
	MovingAverage(Demux_Float_1_out3, Moving_Average_1_out, 50, Mem5, Mem6);
	Square_1_out[0] = pow(Moving_Average_1_out[0], 2);
	Add_3_out[0] = Square_3_out[0] + Square_1_out[0];
	Square_Root_1_out[0] = sqrt(Add_3_out[0]);
	Arctan2_2_out[0] = atan2f(Moving_Average_4_out[0], Square_Root_1_out[0]);
	Rad2Dec_2_out[0] = (180.0f * Arctan2_2_out[0]) / 3.14159265f;
	Square_2_out[0] = pow(Moving_Average_4_out[0], 2);
	Add_2_out[0] = Square_2_out[0] + Square_1_out[0];
	Square_Root_2_out[0] = sqrt(Add_2_out[0]);
	Arctan2_3_out[0] = atan2f(Moving_Average_2_out[0], Square_Root_2_out[0]);
	Rad2Dec_3_out[0] = (180.0f * Arctan2_3_out[0]) / 3.14159265f;
	Add_1_out[0] = Square_3_out[0] + Square_2_out[0];
	Square_Root_3_out[0] = sqrt(Add_1_out[0]);
	Arctan2_1_out[0] = atan2f(Square_Root_3_out[0], Moving_Average_1_out[0]);
	Rad2Dec_1_out[0] = (180.0f * Arctan2_1_out[0]) / 3.14159265f;
	Mux_3_float(Rad2Dec_3_out, Rad2Dec_2_out, Rad2Dec_1_out, Mux_Float_2_out);
	Demux_3_float(Mux_Float_2_out, Demux_Float_2_out1, Demux_Float_2_out2, Demux_Float_2_out3);
	if (Rad2Dec_1_out[0] < Constant_Float_2_out[0]) Less_than_2_out[0] = 1; else Less_than_2_out[0] = 0;
	if (Rad2Dec_2_out[0] < Constant_Float_1_out[0]) Less_than_1_out[0] = 1; else Less_than_1_out[0] = 0;
	And_1_out[0] = Less_than_1_out[0] && Less_than_2_out[0];
	SignalSwitchFloat(Constant_Float_7_out, Demux_Float_2_out1, And_1_out, Switch_Float_1_out);
	SignalSwitchFloat(Constant_Float_9_out, Demux_Float_2_out2, And_1_out, Switch_Float_2_out);
	SignalSwitchFloat(Constant_Float_10_out, Demux_Float_2_out3, And_1_out, Switch_Float_3_out);
	Mux_3_float(Switch_Float_1_out, Switch_Float_2_out, Switch_Float_3_out, Mux_Float_1_out);
	Temperature_Sensor_GetData(Sensor_Hub_1_out, Temperature_C_1_data);
	if (Rad2Dec_2_out[0] < Constant_Float_3_out[0]) Less_than_3_out[0] = 1; else Less_than_3_out[0] = 0;
	if (Rad2Dec_1_out[0] > Constant_Float_4_out[0]) Greater_than_1_out[0] = 1; else Greater_than_1_out[0] = 0;
	And_2_out[0] = Less_than_3_out[0] && Greater_than_1_out[0];
	SignalSwitchFloat(Constant_Float_11_out, Temperature_C_1_data, And_2_out, Switch_Float_4_out);
	Humidity_Sensor_GetData(Sensor_Hub_1_out, Humidity_percent_1_data);
	if (Rad2Dec_1_out[0] > Constant_Float_8_out[0]) Greater_than_3_out[0] = 1; else Greater_than_3_out[0] = 0;
	SignalSwitchFloat(Constant_Float_12_out, Humidity_percent_1_data, Greater_than_3_out, Switch_Float_5_out);
	Pressure_Sensor_GetData(Sensor_Hub_1_out, Pressure_hPa_1_data);
	if (Rad2Dec_2_out[0] > Constant_Float_5_out[0]) Greater_than_4_out[0] = 1; else Greater_than_4_out[0] = 0;
	if (Rad2Dec_1_out[0] > Constant_Float_6_out[0]) Greater_than_2_out[0] = 1; else Greater_than_2_out[0] = 0;
	And_3_out[0] = Greater_than_4_out[0] && Greater_than_2_out[0];
	SignalSwitchFloat(Constant_Float_13_out, Pressure_hPa_1_data, And_3_out, Switch_Float_6_out);
	Display_Update(Mux_Float_1_out, &display_info_list[0]);
	Display_Update(Switch_Float_5_out, &display_info_list[1]);
	Display_Update(Switch_Float_4_out, &display_info_list[2]);
	Display_Update(Switch_Float_6_out, &display_info_list[3]);
}
