#include <math.h>
#include "stdio.h"
#include "ble_comm.h"
#include "hal_flash.h"
#include "hal_imu.h"
#include "system.h"
#include "bsp_time.h"
#include "drv_calibration.h"
#include "app_charge.h"
#include "hal_ble_client.h"
#include "hal_led.h"

char calibration_printfbuf[256];

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

#define ERROR_PIN_ON nrf_gpio_pin_write(PIN_LED_RUN,LED_SMALL_ENABLE);
#define ERROR_PIN_OFF nrf_gpio_pin_write(PIN_LED_RUN,LED_SMALL_DISABLE);

//匿名四轴上位机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];
}


	static float Acc_before[3];
	static float accmod=0,accmodbef=0;
	static char cun=0;
static unsigned int  timer_cli=0;
//需要200ms执行一次,开始信号检测，检测到返回1，否则返回0
char begin_REC(float *acccli)
{

	float gyrmod = 0;
	gyrmod = (acccli[0]*Acc_before[0]) + (acccli[1]*Acc_before[1]) + (acccli[2]*Acc_before[2]);
	accmod = sqrtf(acccli[0] * acccli[0] + acccli[1] * acccli[1] + acccli[2] * acccli[2]);
	gyrmod = gyrmod/(accmod * accmodbef);
	gyrmod = acosf(gyrmod)*3.14f;
	Acc_before[0]=acccli[0];Acc_before[1]=acccli[1];Acc_before[2]=acccli[2];
	accmodbef=accmod;
	Mahony_PRINT("gyrmod:%f,timer:%d cun:%d  %f\n",gyrmod,TIME_GetTicks()-timer_cli,cun,accmodbef);
	timer_cli=TIME_GetTicks();

	if((gyrmod > 0.25f)&&(gyrmod < 0.75f))
	{
		cun++;
	}else 
	{
//		if(cun>=3)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 > 25.0f) && (gyrmod < 60.0f) && (caiyingcun > 20)){
										temp = temp + 1;
								}
								else{
									if(temp>50)temp--;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_GetData");
								}
								else
								{
//									Mahony_PRINT("ImuCal_GetData temp %d",temp);
								}
								overtime++;
								
								if (overtime * invSampleFreq > 300.0f){
										ImuCal_state = ImuCal_error;
									  Mahony_PRINT("ImuCal overtime 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];
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);
}

uint8_t ImuCalibration_GetState(void)
{
	return ImuCal_state;
}

void ImuCalibration_SetState(uint8_t _state)
{
	ImuCal_state = _state;
}