#include "bexus_sensors.h"


/**----------------variables---------------------------*/
uint16_t samplerate_mpu_mag=10;
uint16_t samplerate_temp_press_gps=1000;

uint32_t counter=0;
uint32_t last_time_mpu_mag_read=0;
uint32_t last_time_temp_press_gps_read=0;
uint32_t last_udp_raw = 0;
_Bool new_Sensor_Data=0;

uint8_t error_read=0;
uint8_t mpu_mag_read=0;
uint8_t temp_press_gps_read=0;
uint32_t dataset_count=0;
extern _Bool heartbeat;

double gewichtung = 0.001;
double g = 9.81;
double magdec=   0.17;   //magnetic declination + compass shows to the right - compass shows to the left

double Pi= 3.14159;
double rx_h= 0.001; // 0.001 kalman weight for height vector h
double rx_n= 0.01;  // 0.01  kalman weight for  north vector n
double rn= 0.05;  // p filter  

double Div180Pi, R180Pi;
uint16_t last_index_temp_press_gps =0;
/**-----------------------------------------------------*/


/**------------------functions--------------------------*/
void Sensors_Init(void){
	//with DMA
	SD_Init();
	Gps_Init();

	//without DMA
	Pressure_Cis_Init();	
	Pressure_Spi_Init();
	Temperature_I2c_Init();
	Adc_Init();
	//I2c_Sensors_Init();
	Mag_Init();
	Mpu_Init();
	
	float Earth_R = 6371000; // Radius earth

   Div180Pi= 180 / Pi;
   R180Pi= Earth_R / Div180Pi;
}


int Sensors(void){
		counter=millis();
	error_read = 0;
	if(counter-last_time_mpu_mag_read>=samplerate_mpu_mag){ //100Hz Samplerate
		
		last_time_mpu_mag_read=counter;
		if(HAL_OK != Bexus_Mpu_Read()||Mag_Read())
			error_read=1;
		
		mpu_mag_read=1;
		calc_imu();
		dataset_count++;
		Udp_Send_Raw();
	} 

	if(millis()-last_time_temp_press_gps_read>=samplerate_temp_press_gps ){//1Hz Samplerate
		
		if(HAL_OK != (Gps_Receive() || Pressure_Cis_Read() || Pressure_Spi_Read() || Temperature_I2c_Read() || Adc_Read()))
		   error_read=1;
		//HAL_Delay(2);
		
		temp_press_gps_read=1;
	}

	//counter=millis();

	if(mpu_mag_read == 1 ){
		mpu_mag_read=0;
		new_Sensor_Data=1;
		last_time_mpu_mag_read=counter;
	}
		if(temp_press_gps_read == 1){
		temp_press_gps_read=0;
		new_Sensor_Data=1;
		last_time_temp_press_gps_read=buff_sensor_data_index;
	} 

	
	

	if(new_Sensor_Data==1){
				
		//if(error_read==1) 
		//	return HAL_ERROR;
		
		Dataset_Master[buff_sensor_data_index].count = counter;
			//crc
		Dataset_Master[buff_sensor_data_index].crc=0;
		
		for(int i=0; i<sizeof(Dataset_Master[buff_sensor_data_index]) - 4; i++){
			uint8_t *b = (uint8_t*)&Dataset_Master[buff_sensor_data_index] + i;
			Dataset_Master[buff_sensor_data_index].crc += *b;
		}
		
		
		if(millis() - last_udp_raw > 200) {
			last_udp_raw = millis();
			Udp_Send_Ground();
		}
		
		
		
		SD_Write();
		
		uint32_t old_buff_sensor_data_index = buff_sensor_data_index;
		buff_sensor_data_index = (buff_sensor_data_index +1) % 100;

		memcpy(&Dataset_Master[buff_sensor_data_index],&Dataset_Master[old_buff_sensor_data_index],sizeof(Dataset_Master[0]));
		
		new_Sensor_Data=0;
	}
	
	if(heartbeat==1){
		Fusion();
		heartbeat=0;
	}

	return HAL_OK;
}

struct vector *h_meas, *g_sens, *magnet;
int32_t *mag_head;
	
struct vector h, n, o; // rotation
double res_h, res_n;
double nk, res_n_meas;
struct vector h_est, h_diff,  n_est, n_meas , a, v, pos;
struct vector tm;
double delta_micros = 0.01, oldMicros=0; 

void calc_imu() {
	delta_micros = (millis() - oldMicros) / 1000.0;
	oldMicros = millis();
	
	h_meas = &Dataset_Master[buff_sensor_data_index].accel;
	g_sens = &Dataset_Master[buff_sensor_data_index].gyro;
	magnet = &Dataset_Master[buff_sensor_data_index].mag;
	mag_head = &Dataset_Master[buff_sensor_data_index].mag_head;
	
	//h = &Dataset_Master[buff_sensor_data_index].rot;
	
	h_diff.x = g_sens->x * delta_micros / DIV_180_PI;	//°
	h_diff.y = g_sens->y * delta_micros / DIV_180_PI;
	h_diff.z = g_sens->z * delta_micros / DIV_180_PI;
	
	 // estimation vector h
	
	h_est.x = 			  		h.x	- h_diff.z 	* h.y - h_diff.y 	* h.z; //XYZ Vektor h
	h_est.y = h_diff.z 	* h.x 	+ 						h.y - h_diff.x 	* h.z;
	h_est.z = h_diff.y 	*	h.x	+ h_diff.x *  h.y + 							h.z;
	
	res_h = sqrt(h_est.x * h_est.x + h_est.y * h_est.y + h_est.z * h_est.z);
	
	if(res_h > 0.001){
	h_est.x = h_est.x / res_h;
	h_est.y = h_est.y / res_h;
	h_est.z = h_est.z / res_h;		
	}
	/*	
	h->x = (1 - gewichtung) * h_est.x + gewichtung * h_meas->x;
	h->y = (1 - gewichtung) * h_est.y + gewichtung * h_meas->y;
	h->z = (1 - gewichtung) * h_est.z + gewichtung * h_meas->z;
	*/
		// estimation vector n   
		
	n_est.x = 						n.x	- h_diff.z 	* n.y - h_diff.x 	* n.z;
	n_est.y = h_diff.z 	* n.x 	+ 						n.y - h_diff.y 	* n.z;
	n_est.z = h_diff.x 	*	n.x	+ h_diff.y  * n.y + 							n.z;
	
	res_n = sqrt(n_est.x * n_est.x + n_est.y * n_est.y + n_est.z * n_est.z);	
	
	if(res_n > 0.001){
	n_est.x = n_est.x / res_n;
	n_est.y = n_est.y / res_n;
	n_est.z = n_est.z / res_n;		
	}	
	
	// nk is scaling factor for n based on the 70 deg (Kiruna dec = 77.48333° --> 12,51667° between gravity and mag. north) angle between gravity and magnet vector nk is about 0.94 (Kiruna = 0.97623)
   //res= sqrt(hx*hx + hy*hy + hz*hz);
   //nk = Projektion von Vektor n auf h
   //                  (Skalarprodukt)                           / Betrag
   if (res_h > 0.01) nk= (magnet->x*h.x + magnet->y*h.y + magnet->z*h.z) / (res_h * res_h);
   else nk= 0;
   
   // vector to the magnetic north direction = y-global   
   tm.x= magnet->x - nk*h.x;
   tm.y= magnet->y - nk*h.y;
   tm.z= magnet->z - nk*h.z;
   
   // vector to the geographic north direction - correction of local declination (Kiruna = 9.6°)
   n_meas.x= cos(magdec) * tm.x - sin(magdec) * tm.y;
   n_meas.y= sin(magdec) * tm.x + cos(magdec) * tm.y;
   n_meas.z= tm.z;

   //Normierung
   res_n_meas= sqrt(n_meas.x*n_meas.x + n_meas.y*n_meas.y + n_meas.z*n_meas.z);
   if (res_n_meas > 0.01)
    {
      n_meas.x= n_meas.x / res_n_meas;
      n_meas.y= n_meas.z / res_n_meas;
      n_meas.z= n_meas.z / res_n_meas;
    }
		
	
	 h.x= (1-rx_h) * h_est.x + rx_h * h_meas->x;
   h.y= (1-rx_h) * h_est.y + rx_h * h_meas->y;
   h.z= (1-rx_h) * h_est.z + rx_h * h_meas->z;

   n.x= (1-rx_n) * n_est.x + rx_n * n_meas.x;
   n.y= (1-rx_n) * n_est.y + rx_n * n_meas.y;
   n.z= (1-rx_n) * n_est.z + rx_n * n_meas.z;
   
		if(n.y!=0)
   *mag_head= atan2(-n.x, n.y) * Div180Pi; 
   //if (*mag_head < 0)
		//	 *mag_head= *mag_head + 360;
   
   // vector to the east direction = x-global
	 /*
   o.x= n.y*h.z-n.z*h.y;
   o.y= n.z*h.x-n.x*h.z;
   o.z= n.x*h.y-n.y*h.x; 
		*/
	 o.x = h.y * n.z - h.z * n.y;
	 o.y = h.z * n.x - h.x * n.z;
	 o.z = h.x * n.y - h.y * n.x;
	 
   // global acceleration in the world system in g
   a.x= h_meas->x*o.x + h_meas->y*o.y + h_meas->z*o.z;
   a.y= h_meas->x*n.x + h_meas->y*n.y + h_meas->z*n.z;
   a.z= h_meas->x*h.x + h_meas->y*h.y + h_meas->z*h.z + 1;   
    
   //integration from acceleration to speed      
   v.x= v.x + delta_micros * a.x * g; 
   v.y= v.y + delta_micros * a.y * g;
   v.z= v.z + delta_micros * a.z * g;
    
   //integration from speed to position      
   pos.x = pos.x + delta_micros * v.x; 
   pos.y = pos.y + delta_micros * v.y;
   pos.z = pos.z + delta_micros * v.z;
	
	memcpy(&Dataset_Master[buff_sensor_data_index].rot, &h, sizeof(h));
	
	memcpy(&Dataset_Master[buff_sensor_data_index].global_accel, &a, sizeof(a));
	memcpy(&Dataset_Master[buff_sensor_data_index].pos, &pos, sizeof(pos));	

}



int Fusion(void){//Datafusion Master-Slave
//Dataset_Fusion[buff_sensor_data_index]. = (Dataset_Master[buff_sensor_data_index]. + Dataset_Slave[buff_sensor_data_index].)/2;	
	
	/*-------RAW-----------*/
	Dataset_Fusion[buff_sensor_data_index].pressure_cis_raw = (Dataset_Master[buff_sensor_data_index].pressure_cis_raw + Dataset_Slave[buff_sensor_data_index].pressure_cis_raw)/2;
	Dataset_Fusion[buff_sensor_data_index].temperature_cis_raw = (Dataset_Master[buff_sensor_data_index].temperature_cis_raw + Dataset_Slave[buff_sensor_data_index].temperature_cis_raw)/2;
	
	Dataset_Fusion[buff_sensor_data_index].pressure_spi_raw = (Dataset_Master[buff_sensor_data_index].pressure_spi_raw + Dataset_Slave[buff_sensor_data_index].pressure_spi_raw)/2;
	Dataset_Fusion[buff_sensor_data_index].temperature_i2c_raw = (Dataset_Master[buff_sensor_data_index].temperature_i2c_raw + Dataset_Slave[buff_sensor_data_index].temperature_i2c_raw)/2;
	
	Dataset_Fusion[buff_sensor_data_index].temperature_spi_raw = (Dataset_Master[buff_sensor_data_index].temperature_spi_raw + Dataset_Slave[buff_sensor_data_index].temperature_spi_raw)/2;
	Dataset_Fusion[buff_sensor_data_index].adc_temperature_raw = (Dataset_Master[buff_sensor_data_index].adc_temperature_raw + Dataset_Slave[buff_sensor_data_index].adc_temperature_raw)/2;
	
	Dataset_Fusion[buff_sensor_data_index].temperature_mpu_raw = (Dataset_Master[buff_sensor_data_index].temperature_mpu_raw + Dataset_Slave[buff_sensor_data_index].temperature_mpu_raw)/2;
	Dataset_Fusion[buff_sensor_data_index].adc_temperature_internal_raw = (Dataset_Master[buff_sensor_data_index].adc_temperature_internal_raw + Dataset_Slave[buff_sensor_data_index].adc_temperature_internal_raw)/2;
	
	Dataset_Fusion[buff_sensor_data_index].adc_voltage_raw = (Dataset_Master[buff_sensor_data_index].adc_voltage_raw + Dataset_Slave[buff_sensor_data_index].adc_voltage_raw)/2;
	Dataset_Fusion[buff_sensor_data_index].adc_current_raw = (Dataset_Master[buff_sensor_data_index].adc_current_raw + Dataset_Slave[buff_sensor_data_index].adc_current_raw)/2;
	
	Dataset_Fusion[buff_sensor_data_index].accel_raw.x = (Dataset_Master[buff_sensor_data_index].accel_raw.x + Dataset_Slave[buff_sensor_data_index].accel_raw.x)/2;
	Dataset_Fusion[buff_sensor_data_index].accel_raw.y = (Dataset_Master[buff_sensor_data_index].accel_raw.y + Dataset_Slave[buff_sensor_data_index].accel_raw.y)/2;
	Dataset_Fusion[buff_sensor_data_index].accel_raw.z = (Dataset_Master[buff_sensor_data_index].accel_raw.z + Dataset_Slave[buff_sensor_data_index].accel_raw.z)/2;
	Dataset_Fusion[buff_sensor_data_index].gyro_raw.x = (Dataset_Master[buff_sensor_data_index].gyro_raw.x + Dataset_Slave[buff_sensor_data_index].gyro_raw.x)/2;
	Dataset_Fusion[buff_sensor_data_index].gyro_raw.y = (Dataset_Master[buff_sensor_data_index].gyro_raw.y + Dataset_Slave[buff_sensor_data_index].gyro_raw.y)/2;
	Dataset_Fusion[buff_sensor_data_index].gyro_raw.z = (Dataset_Master[buff_sensor_data_index].gyro_raw.z + Dataset_Slave[buff_sensor_data_index].gyro_raw.z)/2;	
	/*---------------------*/
	
	/*-----Computed--------*/
	Dataset_Fusion[buff_sensor_data_index].pressure_cis = (Dataset_Master[buff_sensor_data_index].pressure_cis + Dataset_Slave[buff_sensor_data_index].pressure_cis)/2;	
	
	Dataset_Fusion[buff_sensor_data_index].temperature_cis = (Dataset_Master[buff_sensor_data_index].temperature_cis + Dataset_Slave[buff_sensor_data_index].temperature_cis)/2;	
	
	Dataset_Fusion[buff_sensor_data_index].pressure_spi = (Dataset_Master[buff_sensor_data_index].pressure_spi + Dataset_Slave[buff_sensor_data_index].pressure_spi)/2;	
	
	Dataset_Fusion[buff_sensor_data_index].temperature_i2c = (Dataset_Master[buff_sensor_data_index].temperature_i2c + Dataset_Slave[buff_sensor_data_index].temperature_i2c)/2;	
	
	Dataset_Fusion[buff_sensor_data_index].temperature_spi = (Dataset_Master[buff_sensor_data_index].temperature_spi + Dataset_Slave[buff_sensor_data_index].temperature_spi)/2;	
	
	Dataset_Fusion[buff_sensor_data_index].adc_temperature = (Dataset_Master[buff_sensor_data_index].adc_temperature + Dataset_Slave[buff_sensor_data_index].adc_temperature)/2;	
	
	Dataset_Fusion[buff_sensor_data_index].adc_temperature_internal = (Dataset_Master[buff_sensor_data_index].adc_temperature_internal + Dataset_Slave[buff_sensor_data_index].adc_temperature_internal)/2;	
	
	Dataset_Fusion[buff_sensor_data_index].adc_voltage = (Dataset_Master[buff_sensor_data_index].adc_voltage + Dataset_Slave[buff_sensor_data_index].adc_voltage)/2;	
	
	Dataset_Fusion[buff_sensor_data_index].adc_current = (Dataset_Master[buff_sensor_data_index].adc_current + Dataset_Slave[buff_sensor_data_index].adc_current)/2;	
	
	Dataset_Fusion[buff_sensor_data_index].temperature_mpu = (Dataset_Master[buff_sensor_data_index].temperature_mpu + Dataset_Slave[buff_sensor_data_index].temperature_mpu)/2;	
	
	Dataset_Fusion[buff_sensor_data_index].accel.x = (Dataset_Master[buff_sensor_data_index].accel.x + Dataset_Slave[buff_sensor_data_index].accel.x)/2;
	Dataset_Fusion[buff_sensor_data_index].accel.y = (Dataset_Master[buff_sensor_data_index].accel.y + Dataset_Slave[buff_sensor_data_index].accel.y)/2;
	Dataset_Fusion[buff_sensor_data_index].accel.z = (Dataset_Master[buff_sensor_data_index].accel.z + Dataset_Slave[buff_sensor_data_index].accel.z)/2;
	Dataset_Fusion[buff_sensor_data_index].gyro.x = (Dataset_Master[buff_sensor_data_index].gyro.x + Dataset_Slave[buff_sensor_data_index].gyro.x)/2;
	Dataset_Fusion[buff_sensor_data_index].gyro.y = (Dataset_Master[buff_sensor_data_index].gyro.y + Dataset_Slave[buff_sensor_data_index].gyro.y)/2;
	Dataset_Fusion[buff_sensor_data_index].gyro.z = (Dataset_Master[buff_sensor_data_index].gyro.z + Dataset_Slave[buff_sensor_data_index].gyro.z)/2;	
	Dataset_Fusion[buff_sensor_data_index].rot.x = (Dataset_Master[buff_sensor_data_index].rot.x + Dataset_Slave[buff_sensor_data_index].rot.x)/2;
	Dataset_Fusion[buff_sensor_data_index].rot.y = (Dataset_Master[buff_sensor_data_index].rot.y + Dataset_Slave[buff_sensor_data_index].rot.y)/2;
	Dataset_Fusion[buff_sensor_data_index].rot.z = (Dataset_Master[buff_sensor_data_index].rot.z + Dataset_Slave[buff_sensor_data_index].rot.z)/2;		
	/*---------------------*/
	
	/*-----------gps------------*/
	Dataset_Fusion[buff_sensor_data_index].longitude = (Dataset_Master[buff_sensor_data_index].longitude + Dataset_Slave[buff_sensor_data_index].longitude)/2;	
	Dataset_Fusion[buff_sensor_data_index].latitude = (Dataset_Master[buff_sensor_data_index].latitude + Dataset_Slave[buff_sensor_data_index].latitude)/2;	
	
	Dataset_Fusion[buff_sensor_data_index].altitude = (Dataset_Master[buff_sensor_data_index].altitude + Dataset_Slave[buff_sensor_data_index].altitude)/2;	
	Dataset_Fusion[buff_sensor_data_index].time = (Dataset_Master[buff_sensor_data_index].time + Dataset_Slave[buff_sensor_data_index].time)/2;	

	Dataset_Fusion[buff_sensor_data_index].hdop = (Dataset_Master[buff_sensor_data_index].hdop + Dataset_Slave[buff_sensor_data_index].hdop)/2;	
	Dataset_Fusion[buff_sensor_data_index].mag_head = (Dataset_Master[buff_sensor_data_index].mag_head + Dataset_Slave[buff_sensor_data_index].mag_head)/2;	
	/*--------------------------*/	

	/*---------magnet-----------*/
	Dataset_Fusion[buff_sensor_data_index].mag_raw.x = (Dataset_Master[buff_sensor_data_index].mag_raw.x + Dataset_Slave[buff_sensor_data_index].mag_raw.x)/2;
	Dataset_Fusion[buff_sensor_data_index].mag_raw.y = (Dataset_Master[buff_sensor_data_index].mag_raw.y + Dataset_Slave[buff_sensor_data_index].mag_raw.y)/2;
	Dataset_Fusion[buff_sensor_data_index].mag_raw.z = (Dataset_Master[buff_sensor_data_index].mag_raw.z + Dataset_Slave[buff_sensor_data_index].mag_raw.z)/2;	
	
	Dataset_Fusion[buff_sensor_data_index].temperature_mag_raw = (Dataset_Master[buff_sensor_data_index].temperature_mag_raw + Dataset_Slave[buff_sensor_data_index].temperature_mag_raw)/2;	

	Dataset_Fusion[buff_sensor_data_index].mag.x = (Dataset_Master[buff_sensor_data_index].mag.x + Dataset_Slave[buff_sensor_data_index].mag.x)/2;
	Dataset_Fusion[buff_sensor_data_index].mag.y = (Dataset_Master[buff_sensor_data_index].mag.y + Dataset_Slave[buff_sensor_data_index].mag.y)/2;
	Dataset_Fusion[buff_sensor_data_index].mag.z = (Dataset_Master[buff_sensor_data_index].mag.z + Dataset_Slave[buff_sensor_data_index].mag.z)/2;		

	Dataset_Fusion[buff_sensor_data_index].temperature_mag = (Dataset_Master[buff_sensor_data_index].temperature_mag + Dataset_Slave[buff_sensor_data_index].temperature_mag)/2;	
	/*--------------------------*/
	
	return HAL_OK;
}	
/*-----------------------------------------------------*/