#include <math.h>
#include "stdio.h"
#include "ble_comm.h"
#include "hal_flash.h"
#include "hal_imu.h"
#include "hal_step.h"
#include "system.h"
#include "bsp_time.h"
#include "app_ImuCalibration.h"
#include "app_charge.h"
#include "hal_ble_client.h"
#include "bmi160_support.h"
#include "hal_attitude.h"

#if CALIBRATION_ENANBLE

enum{
	ImuCal_init,
	ImuCal_GetData,
	ImuCal_Analyze,
	ImuCal_finish,
	ImuCal_error,
  ImuCal_quiet,
};

char printfbuf[256];
void send_ANO(unsigned char fun, unsigned char* p, int len);
#define Mahony_PRINT(...) send_ANO(0,(unsigned char*)printfbuf,sprintf(printfbuf,__VA_ARGS__))
#define ERROR_PIN_ON nrf_gpio_pin_write(PIN_LED_RUN,0);
#define ERROR_PIN_OFF nrf_gpio_pin_write(PIN_LED_RUN,1);

static float invSampleFreq = 0.010f;                                     //采样率(Hz)

//匿名四轴上位机api
void send_ANO(unsigned char fun, unsigned char* p, int len){
    unsigned char buf[256];
    int L = 0;
    unsigned char ver = 0;
    buf[L] = 0xAA;
    ver += buf[L++];
    buf[L] = 0x05;
    ver += buf[L++];
    buf[L] = 0xAF;
    ver += buf[L++];
    buf[L] = fun;
    ver += buf[L++];
    buf[L] = len;
    ver += buf[L++];
    for (int i = 0; i < len; i++)
    {
        buf[L] = p[i];
        ver += buf[L++];
    }
    buf[L++] = ver;
    //    extern void send_bytes_client(unsigned char* bytes, int len);
    send_bytes_client(buf, L);
    //      SEGGER_RTT_Write(0,buf, L);
    //  ESB_SendBuff(buf,L);
}

void send_ANO_Quaternion(float* Q){
    unsigned char buf[256];
    unsigned char L = 0;
    int quat[4];
    quat[0] = Q[0] * 10000;
    quat[1] = Q[1] * 10000;
    quat[2] = Q[2] * 10000;
    quat[3] = Q[3] * 10000;
    buf[L++] = (unsigned char)(quat[0] >> 24);
    buf[L++] = (unsigned char)(quat[0] >> 16);
    buf[L++] = (unsigned char)(quat[0] >> 8);
    buf[L++] = (unsigned char)(quat[0] >> 0);
    buf[L++] = (unsigned char)(quat[1] >> 24);
    buf[L++] = (unsigned char)(quat[1] >> 16);
    buf[L++] = (unsigned char)(quat[1] >> 8);
    buf[L++] = (unsigned char)(quat[1] >> 0);
    buf[L++] = (unsigned char)(quat[2] >> 24);
    buf[L++] = (unsigned char)(quat[2] >> 16);
    buf[L++] = (unsigned char)(quat[2] >> 8);
    buf[L++] = (unsigned char)(quat[2] >> 0);
    buf[L++] = (unsigned char)(quat[3] >> 24);
    buf[L++] = (unsigned char)(quat[3] >> 16);
    buf[L++] = (unsigned char)(quat[3] >> 8);
    buf[L++] = (unsigned char)(quat[3] >> 0);
    buf[L++] = 0;
    send_ANO(0x03, buf, L);
}

void send_ANO_STATUS(float _roll, float _pitch, float _yaw, float _posx, float _posy, float _posz){
    unsigned char buf[256];
    unsigned char L = 0;
    short roll = _roll * 100;
    short pitch = _pitch * 100;
    short yaw = _yaw * 100;
    short posx = _posx * 100;
    short posy = _posy * 100;
    short posz = _posz * 100;
    buf[L++] = (unsigned char)(roll >> 8);
    buf[L++] = (unsigned char)(roll >> 0);
    buf[L++] = (unsigned char)(pitch >> 8);
    buf[L++] = (unsigned char)(pitch >> 0);
    buf[L++] = (unsigned char)(yaw >> 8);
    buf[L++] = (unsigned char)(yaw >> 0);
    buf[L++] = (unsigned char)(posx >> 8);
    buf[L++] = (unsigned char)(posx >> 0);
    buf[L++] = (unsigned char)(posy >> 8);
    buf[L++] = (unsigned char)(posy >> 0);
    buf[L++] = (unsigned char)(posz >> 8);
    buf[L++] = (unsigned char)(posz >> 0);
    buf[L++] = 0;
    send_ANO(0x01, buf, L);
}

void send_ANO_SENSER(short gx, short gy, short gz, short ax, short ay, short az, short mx, short my, short mz){
    unsigned char buf[256];
    unsigned char L = 0;
    buf[L++] = (unsigned char)(ax >> 8);
    buf[L++] = (unsigned char)(ax >> 0);
    buf[L++] = (unsigned char)(ay >> 8);
    buf[L++] = (unsigned char)(ay >> 0);
    buf[L++] = (unsigned char)(az >> 8);
    buf[L++] = (unsigned char)(az >> 0);
    buf[L++] = (unsigned char)(gx >> 8);
    buf[L++] = (unsigned char)(gx >> 0);
    buf[L++] = (unsigned char)(gy >> 8);
    buf[L++] = (unsigned char)(gy >> 0);
    buf[L++] = (unsigned char)(gz >> 8);
    buf[L++] = (unsigned char)(gz >> 0);
    buf[L++] = (unsigned char)(mx >> 8);
    buf[L++] = (unsigned char)(mx >> 0);
    buf[L++] = (unsigned char)(my >> 8);
    buf[L++] = (unsigned char)(my >> 0);
    buf[L++] = (unsigned char)(mz >> 8);
    buf[L++] = (unsigned char)(mz >> 0);
    send_ANO(0x02, buf, L);
}

//LDLT分解法解线性方程组，和LDLTBKSB_6一起用
char LDLTDCMP_6(int n, float (*a)[6]){
    int k;
    int m;
    int i;
    for (k = 0; k < n; k++){
        for (m = 0; m < k; m++){
            a[k][k] = a[k][k] - a[m][k] * a[k][m];
        }
        if (a[k][k] == 0){
            return 1;//error
        }
        for(i = k + 1; i < n; i++){
            for (m = 0; m < k; m++){
                a[k][i] = a[k][i] - a[m][i] * a[k][m];
            }
            a[i][k] = a[k][i] / a[k][k];
        }
    }
    return 0;
}

//LDLT分解法解线性方程组，和LDLTDCMP_6一起用
void LDLTBKSB_6(int n, float (*a)[6], float* b)
{
    int k;
    int i;
    for (i = 0; i < n; i++){
        for (k = 0; k < i; k++){
            b[i] = b[i] - a[i][k] * b[k];
        }
    }
    for (i = n - 1; i >= 0; i--){
        b[i] = b[i] / a[i][i];
        for (k = i + 1; k < n; k++){
            b[i] = b[i] - a[k][i] * b[k];
        }
    }
}

float Acc_static_calibration_b[6] = {0.0f};
float Acc_static_calibration_D[6][6] = {0.0f};
float Acc_static_calibration_out[6] = {1.000718f, 1.001100f, 0.988632f, 0.012943f, 0.006423f, 0.0034f}; //V1.3板

void Ellipsoidfit_six_pram_update(float X, float Y, float Z, float* b, float (*D)[6]){
    float coft[6] = {0.0f};
    int  r, j;
    coft[0] = X * X;
    coft[3] = 2.0f * X;
    coft[1] = Y * Y;
    coft[4] = 2.0f * Y;
    coft[2] = Z * Z;
    coft[5] = 2.0f * Z;
    for (j = 0; j < 6; j++){
        b[j] += coft[j];
    }
    for (r = 0; r < 6; r++){
        for (j = 0; j <= r; j++){
            D[r][j] += coft[r] * coft[j];
        }
    }
    for (r = 0; r < 6; r++){
        for (j = 5; j > r; j--){
            D[r][j] = D[j][r];
        }
    }
}

void Ellipsoidfit_six_pram_Solution(float* b, float (*D)[6], float* out)
{
    float temp = 0;
    //解线性方程组，求解超定方程最小二乘解
    LDLTDCMP_6(6, D);
    LDLTBKSB_6(6, D, b);
    temp = b[1] * b[2] * b[3] * b[3] + b[0] * b[2] * b[4] * b[4] + b[0] * b[1] * b[5] * b[5];
    temp = 1.0f - (temp / (temp + (b[0] * b[1] * b[2])));
    out[0] = sqrtf(temp * b[0]);
    out[1] = sqrtf(temp * b[1]);
    out[2] = sqrtf(temp * b[2]);
    out[3] = b[3] * temp / out[0];
    out[4] = b[4] * temp / out[1];
    out[5] = b[5] * temp / out[2];
}

//需要200ms执行一次,开始信号检测，检测到返回1，否则返回0
char begin_REC(float *acc)
{
	static float Acc_before[3];
	static float accmod,accmodbef;
	static char cun=0;
	float gyrmod = acc[0]*Acc_before[0]+acc[1]*Acc_before[1]+acc[2]*Acc_before[2];
	accmod = sqrtf(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2]);
	gyrmod=gyrmod/(accmod*accmodbef);
	gyrmod=acos(gyrmod)*3.14;
	Acc_before[0]=acc[0];Acc_before[1]=acc[1];Acc_before[2]=acc[2];
	accmodbef=accmod;
	Mahony_PRINT("accmodbef:%f,gyrmod:%f,cun:%d\n",accmodbef,gyrmod,cun);
	if((gyrmod > 0.25f)&&(gyrmod < 0.75f))
	{
		cun++;
	}else cun=0;
	if(cun>=10){
		cun=0;
		return 1;
	}
	else return 0;
}

static uint8_t ImuCal_state = ImuCal_init;
char Acc_static_calibration(float* Acc_in, float* Acc_out, const float* gyr)
{
    static int temp = 0;
    static int overtime = 0;
    static int caiyingcun = 0;
    switch (ImuCal_state){
        case ImuCal_init:{//未校准状态
					if(begin_REC(Acc_in)){
							ImuCal_state = ImuCal_GetData;
							caiyingcun = 0;
							overtime = 0;
							for (int i = 0; i < 6; i++){
									Acc_static_calibration_b[i] = 0.0f;
									Acc_static_calibration_D[i][0] = 0.0f;
									Acc_static_calibration_D[i][1] = 0.0f;
									Acc_static_calibration_D[i][2] = 0.0f;
									Acc_static_calibration_D[i][3] = 0.0f;
									Acc_static_calibration_D[i][4] = 0.0f;
									Acc_static_calibration_D[i][5] = 0.0f;
							}
							ERROR_PIN_ON
							Mahony_PRINT("Acc_static_calibration -> start \r\n");
							temp = 0;
					}
        }
        break;
        case ImuCal_GetData:{//采集校准数据状态
        
								float gyrmod = sqrtf(gyr[0] * gyr[0] + gyr[1] * gyr[1] + gyr[2] * gyr[2]);
								if (gyrmod < 18.27f){ //静止检测
										//采样数据
										if(((Acc_in[0]<2.0f)||(Acc_in[0]>-2.0f))&&((Acc_in[1]<2.0f)||(Acc_in[1]>-2.0f))&&((Acc_in[2]<2.0f)||(Acc_in[2]>-2.0f))){
											Ellipsoidfit_six_pram_update(Acc_in[0], Acc_in[1], Acc_in[2], Acc_static_calibration_b, Acc_static_calibration_D);
											caiyingcun++;
											send_ANO_SENSER(gyr[0] * 100, gyr[1] * 100, gyr[2] * 100, Acc_in[0] * 100, Acc_in[1] * 100, Acc_in[2] * 100, 0, 0, 0);	
										}
								}
								//检测结束条件
								if ((gyrmod > 30.0f) && (gyrmod < 60.0f) && (caiyingcun > 20)){
										temp = temp + 1;
								}
								else{
										temp = 0;
								}
								if (temp * invSampleFreq > 2.0f){
										Ellipsoidfit_six_pram_Solution(Acc_static_calibration_b, Acc_static_calibration_D, Acc_static_calibration_out);
										ImuCal_state = ImuCal_Analyze;
									  Mahony_PRINT("ImuCal_state ImuCal_GetData");
								}
								overtime++;
								
								if (overtime * invSampleFreq > 150.0f){
										ImuCal_state = ImuCal_error;
									  Mahony_PRINT("ImuCal_state ImuCal_error");
								}
						}
        break;
        case ImuCal_Analyze:{//校准数据处理状态
								float gyrmod = sqrtf(gyr[0] * gyr[0] + gyr[1] * gyr[1] + gyr[2] * gyr[2]);
								if (gyrmod < 18.27f){ //静止检测
										if (((Acc_static_calibration_out[0] > 0.85f) && (Acc_static_calibration_out[0] < 1.15f)) &&
												((Acc_static_calibration_out[1] > 0.85f) && (Acc_static_calibration_out[1] < 1.15f)) &&
												((Acc_static_calibration_out[2] > 0.85f) && (Acc_static_calibration_out[2] < 1.15f))){
//													Mahony_PRINT("out[%f,%f,%f,%f,%f,%f] \r\n", Acc_static_calibration_out[0], Acc_static_calibration_out[1], Acc_static_calibration_out[2], Acc_static_calibration_out[3], Acc_static_calibration_out[4], Acc_static_calibration_out[5]);
													Mahony_PRINT("ImuCal_state ImuCal_finish");
													temp = 0;
													ImuCal_state = ImuCal_finish;
													for(int a = 0; a < 6; a++){
									            mBackup.cal[a] = Acc_static_calibration_out[a];
								          } 
										}
										else{
												ImuCal_state = ImuCal_quiet;
										}
										overtime = 0;
								}
             }
        break;
//        case ImuCal_finish:{//已经校准完状态
//					    Mahony_PRINT("ImuCal_state ImuCal_init");
//							ImuCal_state = 0;
//            }
//        break;
//        case ImuCal_error:{//校准过程中断退出状态
//					    Mahony_PRINT("ImuCal_state ImuCal_init");
//							ImuCal_state = 0;
//					    Mahony_PRINT("ImuCal_state ImuCal_init");
//            }
//        break;
//        case ImuCal_quiet:{//校准过程中断退出状态
//							ImuCal_state = 0;
//					  }
//        break;
    }
    return ImuCal_state;
}

void Mahony_imu_lbs(short* Acc_in, short* Gyr_in, short* Mag_in, float* Acc, float* Gyr, float* Mag)
{
    float ACC_LBS = 32768.0f / 16.0f;
    float GYR_LBS = 32768.0f / 2000.0f;
    Acc[0] = Acc_in[0] / ACC_LBS;
    Acc[1] = Acc_in[1] / ACC_LBS;
    Acc[2] = Acc_in[2] / ACC_LBS;
    Gyr[0] = Gyr_in[0] / GYR_LBS;
    Gyr[1] = Gyr_in[1] / GYR_LBS;
    Gyr[2] = Gyr_in[2] / GYR_LBS;
    Mag[0] = Mag_in[0] / 1.0f;
    Mag[1] = Mag_in[1] / 1.0f;
    Mag[2] = Mag_in[2] / 1.0f;
}

static float acc[3], gyr[3], mag[3];
static void ImuCalibration_pcs(short* Acc, short* Gyr, short* Mag)
{
    Mahony_imu_lbs(Acc, Gyr, Mag, acc, gyr, mag); //转换IMU数据量程，加速度单位转换为G，陀螺仪单位转换为度每秒，磁力计不需要转换单位，后面会做归一化处理
    Acc_static_calibration(acc, NULL, gyr);
}

/***********************************
 *未校准：灭灯
 *校准中：指示灯常亮
 *校准完成：指示灯慢闪，亮10ms,灭灯2S
 *校准错误：指示灯快闪，亮10ms,灭灯100ms
************************************/
static void app_ImuCalLed_process(void){
		nrf_gpio_pin_write(PIN_LED_RUN,0); 
		nrf_delay_ms(10);
		nrf_gpio_pin_write(PIN_LED_RUN,1);
	  
	  char printdata[200]={0};
	  sprintf(printdata,"caldata:%f,%f,%f,%f,%f,%f",mBackup.cal[0],mBackup.cal[1],mBackup.cal[2],mBackup.cal[3],mBackup.cal[4],mBackup.cal[5]);
	  SEGGER_RTT_printf(0,"%s\n",printdata);
		Mahony_PRINT("caldata:%f,%f,%f,%f,%f,%f",mBackup.cal[0],mBackup.cal[1],mBackup.cal[2],mBackup.cal[3],mBackup.cal[4],mBackup.cal[5]);

}

uint8_t app_imucal_getstate(void){
	 if(ImuCal_GetData == ImuCal_state || ImuCal_Analyze == ImuCal_state)return 1;
	 else return 0;
}

static void app_cal_process(void){
	 static uint8_t state =0;
   static uint8_t temp =0;
	 int16_t Acc[3]={0};
	 int16_t Gry[3]={0};
	 int16_t Mag[3]={0};
	 switch(state){
		 case 0: 
			    IMU_GetAcc(Acc);
          IMU_GetGry(Gry);
//		      SEGGER_RTT_printf(0,"h_ax=%d\r,h_ay=%d\r,h_az=%d\r\n",Acc[0],Acc[1],Acc[2]);
//	        SEGGER_RTT_printf(0,"h_gx=%d\r,h_gy=%d\r,h_gz=%d\r\n",Gry[0],Gry[1],Gry[2]);
					ImuCalibration_pcs((short *)Acc,(short *)Gry,(short *)Mag);
					 if(ImuCal_state == ImuCal_GetData){
							Process_UpdatePeroid(app_cal_process,10);
							Process_SetHoldOn(app_cal_process,1);
						  nrf_gpio_pin_write(PIN_LED_RUN,0);
							hal_attitude_setMode(STANDARD_UI_IMU);
							state =1;
						  temp=0;
					 } 
					 break;
		 case 1:
			     if(1 == hal_attitude_GetMode(STANDARD_UI_IMU)){
						  state =2;
					 }else if(temp++ >=20)state =0;
			     break;
		 case 2:
			    IMU_GetAcc(Acc);
          IMU_GetGry(Gry);
				  ImuCalibration_pcs((short *)Acc,(short *)Gry,(short *)Mag);
//		      SEGGER_RTT_printf(0,">>>>>>>22222\n");
				  if(ImuCal_state == ImuCal_finish){
						Flash_SaveBackup();
						Mahony_PRINT("ImuCal_state ImuCal_finish");
						Process_Start(2000,"app_cal_process",app_ImuCalLed_process);
						Process_UpdatePeroid(app_ImuCalLed_process,2000);
						
						Process_UpdatePeroid(app_cal_process,200);
						Process_SetHoldOn(app_cal_process,0);
						hal_attitude_setMode(ACCEL_PEDOMETER_FIFO);
						state =3;
				  }
				  else if(ImuCal_state == ImuCal_error || ImuCal_state == ImuCal_quiet){
						Mahony_PRINT("ImuCal_state ImuCal_error");
						Process_Start(100,"app_cal_process",app_ImuCalLed_process);
						Process_UpdatePeroid(app_ImuCalLed_process,100);
						
						Process_UpdatePeroid(app_cal_process,200);
						Process_SetHoldOn(app_cal_process,1);
						hal_attitude_setMode(ACCEL_PEDOMETER_FIFO);
						state =3;
				  }
					break;
		 case 3:
			    if(1 == hal_attitude_GetMode(ACCEL_PEDOMETER_FIFO)){
					    state = 4;
				  }
				  else if(++temp>20){ temp = 0;
					    state = 4;
				  }
			    break;
		 case 4:
			    if(app_charge_Getstate() == BLE_Client_T_CHARGE_INSERT ||	app_charge_Getstate() == BLE_Client_T_CHARGE_DONE){
						  state =0;
						  ImuCal_state = ImuCal_init;
						  Process_Stop(app_ImuCalLed_process);
						  nrf_gpio_pin_write(PIN_LED_RUN,1);
					}
			    break;
	 }
}

void app_ImuCalibration_init(void){
	 Process_Start(200,"app_cal_process",app_cal_process);
}

#endif