/* ---------------------------------------------
//ad0x0 mpu6050
* ----------------------------------------------
*/

//#include "stm8s_conf.h"
//#include "stm8s_delay.h"
//#include "i2c_master_poll.h"
#include "../i2c/ad0x0_i2c2.h"
#include "../i2c/ad0x0_i2c_ext.h"
#include <string.h>
#include "mpu6050.h"
#include "mpu6050_r.h"
#include "../admisc/ad0x0_timman.h"

volatile u8 mpu6050_adq_byte=0;

static ad0x0_gyrodata_s gdata14,gdata_be,gdata_le;//be==BigEndian
static bool	g_gyro_tick=false;
static volatile uint32_t last_update_tick=0;
static uint8_t zavis=0;
/*#define MPU6050_POWER_Pin GPIO_PIN_1
#define MPU6050_POWER_GPIO_Port GPIOB*/

void mpu6050_writebits(uint8_t _reg, uint8_t _or_bits, uint8_t _and_mask);

/*
было дело, лечил зависания методом выключить\включить...
void	power_set(bool _isOn){
	#ifdef MPU6050_POWER_GPIO_Port
		HAL_GPIO_WritePin(MPU6050_POWER_GPIO_Port,MPU6050_POWER_Pin,_isOn ? GPIO_PIN_SET : GPIO_PIN_RESET);
	#endif
}*/

static uint32_t get_per_ms(uint32_t _tick_start){
	uint32_t t=(HAL_GetTick()<_tick_start)
		?((uint64_t)HAL_GetTick()+UINT32_MAX-_tick_start)
		:(HAL_GetTick()-_tick_start);
	return t;
}

uint8_t mpu6050_wakeup(){
	if(get_per_ms(last_update_tick)>300){
		zavis=true;
		return 1;//выпилиться 
	}
	return 0;//не выпиливаться из менеджера тиков
}

void mpu6050_stop(void){
	ad0x0_timman_remove(mpu6050_wakeup);
	g_gyro_tick=false;	
	zavis=false;
}

ad0x0_gyrodata_s *mpu6050_get_gyrodata_s(void){
	return &gdata_be;
}

//ad0x0_gyrodata_s *mpu6050_get_gyrodata_s_le(void){	return &gdata_le;}

enum{
	xMPU6050_P_OFF,
	xMPU6050_P_ON,
	xMPU6050_START	
};

void	cb_mpu5060_power_set(uint8_t _sf_id,ad0x0_i2c_desc_s *pdi2c,void *p_i2c_ext_t){
//	ad0x0_i2c_ext_t *p_ext=(ad0x0_i2c_ext_t*)p_i2c_ext_t;
	switch(_sf_id){
		/*case xMPU6050_P_OFF:
			power_set(0);
		break;
		case xMPU6050_P_ON:
			power_set(1);
		break;*/
		case xMPU6050_START:
			g_gyro_tick=true;
			zavis=false;
			last_update_tick=HAL_GetTick();
		//ad0x0_timman_add(10,mpu6050_wakeup);перенес в инит, виснет при инициализации сука
		break;
	}
}

void mpu6050_start(void){
	ad0x0_i2c2_push_ext_setfunc(xMPU6050_START,cb_mpu5060_power_set);
}

void mpu6050_on_i2c_err(void){
	mpu6050_stop();
}

void mpu6050_on_i2c_restart(void){
	mpu6050_init();
	mpu6050_start();
}

void mpu6050_init(void){
		/*power_set(false);
		HAL_Delay(200);
		power_set(true);
		HAL_Delay(100);*/
	ad0x0_i2c2_set_restart_func(mpu6050_on_i2c_err,mpu6050_on_i2c_restart);
	ad0x0_timman_add(10,mpu6050_wakeup);
//	ad0x0_i2c2_push_ext_setfunc(xMPU6050_P_OFF, cb_mpu5060_power_set);
//	ad0x0_i2c2_push(ADQ_DELAY_MS,250);
//	ad0x0_i2c2_push_ext_setfunc(xMPU6050_P_ON, cb_mpu5060_power_set);
//	ad0x0_i2c2_push(ADQ_DELAY_MS,250);
	
	mpu6050_writebits(MPU6050_RA_PWR_MGMT_1,0,~(1<<MPU6050_PWR1_SLEEP_BIT));

  //memset(&gdata,0,sizeof(gdata));	затрет посчитанные данные, пусть остается то шо было
//  mpu6050_writebit(MPU6050_RA_PWR_MGMT_1,MPU6050_PWR1_SLEEP_BIT,0);
  ad0x0_i2c2_push(ADQ_DELAY_MS,100);
	//тут блин ппц
	if(1){
		mpu6050_writebyte(25,16);//sample rate div=8 (8/24)=0.333 kHz
		mpu6050_writebyte(26,0);//filter 
	}else{
		mpu6050_writebyte(25,1);//sample rate div
		mpu6050_writebyte(26,6);//filter 
	}
	//
	mpu6050_writebyte(0x38,0x9);//прерывания
  //mpu6050_readbyte(MPU6050_RA_WHO_AM_I);
  
  //mpu6050_readbytes(MPU6050_RA_ACCEL_XOUT_H,(u8*)&gdata,14);
  ad0x0_i2c2_push(ADQ_DELAY_MS,1);
}


//*********************************************************
static void	cb_write_register_bit2(uint8_t _sf_id,ad0x0_i2c_desc_s *pi2c,void *p_i2c_ext_t);
static void	cb_write_register_bit(uint8_t _sf_id,ad0x0_i2c_desc_s *pi2c,void *p_i2c_ext_t){
	ad0x0_i2c_ext_t *p_ext=(ad0x0_i2c_ext_t*)p_i2c_ext_t;

	p_ext->_p_post_cb_func = cb_write_register_bit2;	
}

static void	cb_write_register_bit2(uint8_t _sf_id,ad0x0_i2c_desc_s *pi2c,void *p_i2c_ext_t){
	ad0x0_i2c_ext_t *p_ext=(ad0x0_i2c_ext_t*)p_i2c_ext_t;
	/*пост процесс команды чтения*/
	uint8_t value =  (uint8_t)(p_ext->user_data1 & 0xFF);//прочитанный байт
	uint8_t reg =  p_ext->user_data0 & 0xFF;
	value &= (uint8_t)(p_ext->user_data0>>24);//_and_mask
	value |= (uint8_t)(p_ext->user_data0>>16);//_or_bits
	//модифицируем ячейку под следующую команду	
	p_ext->user_data0 = (uint32_t)value<<8 | (uint32_t)reg;
	p_ext->pbuf_count=2;//уже 2 байта пишим: рег, знач.
	p_ext->_p_post_cb_func = NULL;
}

void mpu6050_writebits(uint8_t _reg, uint8_t _or_bits, uint8_t _and_mask){
  /*ad0x0_i2c2_push(ADQ_START_ADDR,MPU6050_ADDRESS_W);
  ad0x0_i2c2_push(ADQ_TX,_reg);
  ad0x0_i2c2_push(ADQ_START_ADDR,MPU6050_ADDRESS_R);*/
  

	uint8_t h_ext;	
	ad0x0_i2c_ext_t *p_ext_t=NULL;	
	ad0x0_i2c2_get_ext_async2(&h_ext,(void**)&p_ext_t);//получить ячейку, ассоциированную с командой в очереди
	//дальше полный изврат
	
	p_ext_t->addr_w=MPU6050_ADDRESS_W;
	p_ext_t->addr_r=MPU6050_ADDRESS_R;
	p_ext_t->user_data0=_reg | (uint32_t)0xFF00 | (uint32_t)_or_bits<<16 | (uint32_t)_and_mask<<24;
	p_ext_t->_p_post_cb_func = cb_write_register_bit;
	
	p_ext_t->pbuf_count=1;
	p_ext_t->pbuf_tx=(uint8_t*)&p_ext_t->user_data0;//TX!!!!!!!
	p_ext_t->pbuf_rx=(uint8_t*)&p_ext_t->user_data1;//RX!!!
	p_ext_t->p_pre_SetTXRXBuf=EXT_SetTXRXBuf_i2c;
	p_ext_t->reference_count=2;//после 3й команды высвободится... вроде... механизм тупой, но хоть что-то
	ad0x0_i2c2_push(ADQ_HAL_ADDR_TX_BUF_SEQ_FIRST_EXT,h_ext);
	ad0x0_i2c2_push(ADQ_HAL_ADDR_RX_BUF_SEQ_LAST_EXT,h_ext);
  ad0x0_i2c2_push(ADQ_HAL_ADDR_TX_BUF_STOP_EXT,h_ext);
}


void mpu6050_writebyte(u8 _reg,u8 _byte){
/*  ad0x0_i2c2_push(ADQ_START_ADDR,MPU6050_ADDRESS_W);
  ad0x0_i2c2_push(ADQ_TX,_reg);
  ad0x0_i2c2_push(ADQ_TX_STOP,_byte); */
	uint8_t h_ext;	
	ad0x0_i2c_ext_t *p_ext_t=NULL;	
	ad0x0_i2c2_get_ext_async2(&h_ext,(void**)&p_ext_t);//получить ячейку, ассоциированную с командой в очереди
	
	p_ext_t->addr_w=MPU6050_ADDRESS_W;
	p_ext_t->user_data0=_reg | (uint32_t)(_byte<<8);
	p_ext_t->pbuf_count=2;
	p_ext_t->pbuf_tx=(uint8_t*)&p_ext_t->user_data0;//TX!!!!!!!
	p_ext_t->p_pre_SetTXRXBuf=EXT_SetTXRXBuf_i2c;
  ad0x0_i2c2_push(ADQ_HAL_ADDR_TX_BUF_STOP_EXT,h_ext);	
}

/*void mpu6050_readbyte(u8 _reg){
  ad0x0_i2c2_push(ADQ_START_ADDR,MPU6050_ADDRESS_W);
  ad0x0_i2c2_push(ADQ_TX,_reg);
  ad0x0_i2c2_push(ADQ_START_ADDR,MPU6050_ADDRESS_R);
  ad0x0_i2c2_push(ADQ_RX_STOP,(u8)mpu6050_adq_byte);
}*/
static void	cb_mpu6050_readbytes2(uint8_t _sf_id,ad0x0_i2c_desc_s *pi2c,void *p_i2c_ext_t);
static void	cb_mpu6050_readbytes(uint8_t _sf_id,ad0x0_i2c_desc_s *pi2c,void *p_i2c_ext_t){
	ad0x0_i2c_ext_t *p_ext=(ad0x0_i2c_ext_t*)p_i2c_ext_t;
	p_ext->pbuf_count=p_ext->user_data1;
	p_ext->_p_post_cb_func = cb_mpu6050_readbytes2;
	//refcount больше тут не инкрементируем чтобы все релизилось правильно
}
static void	cb_mpu6050_readbytes2(uint8_t _sf_id,ad0x0_i2c_desc_s *pi2c,void *p_i2c_ext_t){
	//ad0x0_i2c_ext_t *p_ext=(ad0x0_i2c_ext_t*)p_i2c_ext_t;
	//раз мы тут, то все прочиталось, иначе бы была перезагрузка матр.. очереди
	#define AD0X0_6050_LE_SWAP(X,Y) X = ((uint16_t)Y >> 8)|((uint16_t)Y << 8);
	memcpy(&gdata_le,&gdata14,sizeof(ad0x0_gyrodata_s));
	//memcpy(&gdata_be,&gdata,sizeof(ad0x0_gyrodata_s));
	AD0X0_6050_LE_SWAP(gdata_be.ax,gdata_le.ax);
	AD0X0_6050_LE_SWAP(gdata_be.ay,gdata_le.ay);
	AD0X0_6050_LE_SWAP(gdata_be.az,gdata_le.az);
	AD0X0_6050_LE_SWAP(gdata_be.gx,gdata_le.gx);
	AD0X0_6050_LE_SWAP(gdata_be.gy,gdata_le.gy);
	AD0X0_6050_LE_SWAP(gdata_be.gz,gdata_le.gz);
}

//void mpu6050_readbytes(u8 _reg,u8 *_pbuf,u8 _count){//ломаем универсальность.. см. ниже
/*расклад такой, эта скотина иногда зависает и мы ее перезагружаем. Это
не мешает начитать в буфер всякой херни, по этому делаем вот что:
читаем 1) в gdata14 то, что идет с шины, 2) __если__ все прочиталось нормально, то
согласно моему плану, вызовется cb_mpu6050_readbytes2. И там уже переворачиваем байты
для big endian представления*/
void mpu6050_read14(void){
	uint8_t h_ext;	
	ad0x0_i2c_ext_t *p_ext_t=NULL;	
	ad0x0_i2c2_get_ext_async2(&h_ext,(void**)&p_ext_t);//получить ячейку, ассоциированную с командой в очереди
	
	p_ext_t->addr_w=MPU6050_ADDRESS_W;
	p_ext_t->addr_r=MPU6050_ADDRESS_R;
	p_ext_t->user_data0=MPU6050_RA_ACCEL_XOUT_H;
	p_ext_t->user_data1=14;//_count;
	p_ext_t->_p_post_cb_func = cb_mpu6050_readbytes;
	
	p_ext_t->pbuf_count=1;//1 - это адрес регистра, кол-во байт чтения выставим в колбэке
	p_ext_t->pbuf_tx=(uint8_t*)&p_ext_t->user_data0;//TX!!!!!!!
	p_ext_t->pbuf_rx=(uint8_t*)&gdata14;//RX!!!
	p_ext_t->p_pre_SetTXRXBuf=EXT_SetTXRXBuf_i2c;
	p_ext_t->reference_count=1;
	ad0x0_i2c2_push(ADQ_HAL_ADDR_TX_BUF_SEQ_FIRST_EXT,h_ext);
	ad0x0_i2c2_push(ADQ_HAL_ADDR_RX_BUF_SEQ_LAST_EXT,h_ext);

/*  ad0x0_i2c2_push(ADQ_START_ADDR,MPU6050_ADDRESS_W);
  ad0x0_i2c2_push(ADQ_TX,_reg);
  ad0x0_i2c2_push(ADQ_START_ADDR,MPU6050_ADDRESS_R);
	ad0x0_i2c2_push_rx_pbuf_stop_ext(NULL,_pbuf,_count);*/
	
}

/*#define AD0X0_6050_LE_SWAP(X,Y) X = ((uint16_t)Y >> 8)|((uint16_t)Y << 8);
static void mpu6050_damn_lendian(void){
	//gdata.ax=(gdata.ax >> 8)|(gdata.ax << 8);
	memcpy(&gdata_le,&gdata,sizeof(ad0x0_gyrodata_s));
	//memcpy(&gdata_be,&gdata,sizeof(ad0x0_gyrodata_s));
	AD0X0_6050_LE_SWAP(gdata_be.ax,gdata_le.ax);
	AD0X0_6050_LE_SWAP(gdata_be.ay,gdata_le.ay);
	AD0X0_6050_LE_SWAP(gdata_be.az,gdata_le.az);
	AD0X0_6050_LE_SWAP(gdata_be.gx,gdata_le.gx);
	AD0X0_6050_LE_SWAP(gdata_be.gy,gdata_le.gy);
	AD0X0_6050_LE_SWAP(gdata_be.gz,gdata_le.gz);
	//memcpy(&gdata_be,&gdata,sizeof(ad0x0_gyrodata_s));
}*/
void	cb_mpu5060_upd_data(uint8_t _sf_id,ad0x0_i2c_desc_s *pdi2c,void *p_i2c_ext_t){
	//mpu6050_damn_lendian(); перенес в mpu6050_read14.cb_mpu6050_readbytes2
	extern void ad0x0_gyro_datareaded_cb(void);
	ad0x0_gyro_datareaded_cb();
}

void update_gdata(void){
	if(ad0x0_i2c2_is_queue_ready()){
		mpu6050_read14();
		ad0x0_i2c2_push_ext_setfunc(0,cb_mpu5060_upd_data);
		last_update_tick=HAL_GetTick();
	}else{
		/*ошибка периода следования: это когда мы считываем текущие значения, 
		и приходит очередное прерывание*/
		__ASM("nop");
	}	
}

uint32_t mpu6050_get_last_update_tick(void){
	return last_update_tick;
}

uint8_t mpu6050_zavis(void){
	zavis=get_per_ms(last_update_tick)>300;
	return zavis;
}

#define AD_GYRO_EVMAX 2;
//int	g_gyro_evmax=10;
void mpu6050_pin_irq(void){
		if(g_gyro_tick){
			update_gdata();			
		}
}