#include "bexus_mag.h"


// Defines ////////////////////////////////////////////////////////////////
#define LIS3MDL_SA1_HIGH_ADDRESS  0x1e<<1
#define LIS3MDL_SA1_LOW_ADDRESS   0x1c<<1

#define TEST_REG_ERROR -1

#define LIS3MDL_WHO_ID  0x3D


uint8_t address = LIS3MDL_SA1_HIGH_ADDRESS;

//double gauss_scale=1711;//für +-16 gauss

uint8_t xlm, xhm, ylm, yhm, zlm, zhm, tlm, thm;
uint8_t status_reg;
uint8_t reg_zor=0x02, reg_yor=0x04, reg_xor=0x08, reg_zda=0x20, reg_yda=0x40, reg_xda=0x80;

int16_t reg_cal[3][2]={{0,0},{0,0},{0,0}}; // [x,y,z] [min,max]
int16_t reg_cal_offset[2];//offset for [min,max]



void Mag_cal(void) {
	//Mag_Init();
	
	uint8_t temp_buff[6];
	uint8_t val = 0x06;
	uint8_t outxl[2] = {OUT_X_L  | 0x80,val};

for(int i=0;i<500;i++){
		if(HAL_OK == HAL_I2C_Master_Transmit(&hi2c1,address,outxl,2,10))			
			 HAL_I2C_Master_Receive(&hi2c1,address,temp_buff,sizeof(temp_buff),10);
		
		xlm = temp_buff[0];
		xhm = temp_buff[1];
		ylm = temp_buff[2];
		yhm = temp_buff[3];
		zlm = temp_buff[4];
		zhm = temp_buff[5];	


			 
		// combine high and low bytes
		int16_t buff_cal[3] = {(int16_t)((xhm << 8) | xlm) , (int16_t)((yhm << 8) | ylm) , (int16_t)((zhm << 8) | zlm)};	
		
		for(int i=0;i<3;i++) if(buff_cal[i] < reg_cal[i][0])  reg_cal[i][0] = buff_cal[i];
		for(int i=0;i<3;i++) if(buff_cal[i] > reg_cal[i][1])  reg_cal[i][1] = buff_cal[i]; 
			 
		HAL_Delay(10);	
	}	

	reg_cal_offset[0]=(reg_cal[0][0] + reg_cal[1][0] + reg_cal[2][0])/3.;	 
	reg_cal_offset[1]=(reg_cal[0][1] + reg_cal[1][1] + reg_cal[2][1])/3.;		
	
}


void Mag_Init(void) {
	
    // ultra-high-performance mode for X and Y, 155 Hz FAST_ODR
	  //writeReg(CTRL_REG1, 0xE2);
		writeReg(CTRL_REG1, 0xFC);
	
    // +/-  gauss full scale
	  writeReg(CTRL_REG2, 0x60); //+-16
	  //writeReg(CTRL_REG2, 0x20);//+-8
	
    // continuous-conversion mode
    writeReg(CTRL_REG3, 0x00);

	
    // ultra-high-performance mode for Z
    writeReg(CTRL_REG4, 0x0C);
	}


// Writes a mag register
void writeReg(uint8_t reg, uint8_t value){
	uint8_t temp_mag[2]={reg,value};
	HAL_I2C_Master_Transmit(&hi2c1,address,temp_mag,2,50);

}


// Reads a mag register
uint8_t readReg(uint8_t reg){
  uint8_t value;
	
	uint8_t val [2]= {reg,0x01};
	 if(HAL_OK == HAL_I2C_Master_Transmit(&hi2c1,address,val,1,10))				
			HAL_I2C_Master_Receive(&hi2c1,address,&value,1,35);
	
  return value;
}

struct Dataset *data_mag = &Dataset_Master[0];
struct vector_Uint data_mag_save;

// Reads the 3 mag channels
int Mag_Read() {
	
	uint8_t temp_buff[9];
	uint8_t val = 0x09;
	uint8_t outxl[2] = {STATUS_REG  | 0x80,val};


	if(HAL_OK == HAL_I2C_Master_Transmit(&hi2c1,address,outxl,2,50))			
		 if(HAL_OK != HAL_I2C_Master_Receive(&hi2c1,address,temp_buff,sizeof(temp_buff),50))
				return HAL_ERROR;
	
	
	
  status_reg = temp_buff[0];
  xlm = temp_buff[1];
  xhm = temp_buff[2];
  ylm = temp_buff[3];
  yhm = temp_buff[4];
  zlm = temp_buff[5];
  zhm = temp_buff[6];	
	tlm = temp_buff[7];
  thm = temp_buff[8];
	
		 
  // combine high and low bytes
  data_mag->mag_raw.x = (int16_t)((xhm << 8) | xlm);
  data_mag->mag_raw.y = (int16_t)((yhm << 8) | ylm);
  data_mag->mag_raw.z = (int16_t)((zhm << 8) | zlm);
	data_mag->temperature_mag_raw = (int32_t)((thm << 8) | tlm);

  data_mag->temperature_mag_raw = data_mag->temperature_mag_raw - (data_mag->temperature_mag_raw & 0xFF00);

	
	//check for new/overrun data and calculate dataset
//	if(((reg_xor & status_reg) != reg_xor)&&((reg_xda & status_reg) == reg_xda))
//			data_mag->mag.x = (double)data_mag->mag_raw.x; 
//	else
//			data_mag->mag.x = data_mag_prev->mag.x;
//	
//	
//	if(((reg_yor & status_reg) != reg_yor)&&((reg_yda & status_reg) == reg_yda))
//			data_mag->mag.y=(double)(data_mag->mag_raw.y);
//	else
//			data_mag->mag.y = data_mag_prev->mag.y;
//	
//	
//	if(((reg_zor & status_reg) != reg_zor)&&((reg_zda & status_reg) == reg_zda))
//			data_mag->mag.z=(double)(data_mag->mag_raw.z); 
//	else
//			data_mag->mag.z = data_mag_prev->mag.z;

	if(abs(data_mag->mag_raw.x - data_mag_save.x) < 20000) {
		data_mag_save.x = data_mag->mag_raw.x;
	}
	if(abs(data_mag->mag_raw.y - data_mag_save.y) < 20000) {
		data_mag_save.y = data_mag->mag_raw.y;
	}
	if(abs(data_mag->mag_raw.z - data_mag_save.z) < 20000) {
		data_mag_save.z = data_mag->mag_raw.z;
	}

#ifdef BOARD_HAS_ETHERNET
	data_mag->mag.x = - data_mag_save.y + 0.;
	data_mag->mag.y = + data_mag_save.x + 0.;
	data_mag->mag.z = + data_mag_save.z + 0.;
#else
	data_mag->mag.x = - data_mag_save.y + 0.;
	data_mag->mag.y = + data_mag_save.x + 0.;
	data_mag->mag.z = + data_mag_save.z + 0.;
#endif
	
	double temp_temp=(double)((data_mag->temperature_mag_raw)/256.)+25; 
	if(temp_temp<100){//checking for false data, since there is no crc for the temperature register
	data_mag->temperature_mag = temp_temp;
	}
	/*
	if(data_mag->mag_raw.x >= 32768)
		  data_mag->mag.x=(double)((data_mag->mag_raw.x*1.-32768)/32768*16); 
	else
		  data_mag->mag.x=0-(double)((data_mag->mag_raw.x*1.)/32768*16); 
	*/

	
return HAL_OK;	
}

double vector_dot(struct vector *a, struct vector *b) {
  return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
}
void vector_normalize(struct vector *a)
{
  double mag_d = sqrt(vector_dot(a, a));
  a->x /= mag_d;
  a->y /= mag_d;
  a->z /= mag_d;
}


// Private Methods //////////////////////////////////////////////////////////////

uint8_t testReg(uint8_t address, uint8_t reg){ 
	uint8_t val[2] = {reg,0x01};
	uint8_t erg;
	
	if(HAL_OK ==HAL_I2C_Master_Transmit(&hi2c1,address,val,2,50))			
	  	HAL_I2C_Master_Receive(&hi2c1,address,&erg,1,50);
		 
		 
  return erg;
}