/******************** (C) COPYRIGHT 2023 Geek************************************
* File Name          : MagCalibrate.cpp
* Department         : Sensor Algorithm Team
* Current Version    : V1.0
* Author             : zhanli
* Date of Issued     : 2023.09.10
* Comments           : GeekIMU 磁力计校准模块
********************************************************************************/
/* Header File Including -----------------------------------------------------*/

#include <stdio.h>
#include <math.h>
#include <conio.h>
#include "MagCalibrate.h"
#include <fstream>
#include "Log.h"

#define ENABLE_DEBUG_LOG  1

MagData mdata[4000];

// 磁力计的模值检查
bool checkMagnetNorm(float x, float y, float z);

MagCaliParam CalculateMagCalibrationParams(MagData _mdata[], int dataSize)
{
	MagCaliParam param, lastparam;

	srand((unsigned)time(NULL));

	float	xmin = _mdata[0].x;
	float	ymin = _mdata[0].y;
	float	zmin = _mdata[0].z;
	float	xmax = _mdata[0].x;
	float	ymax = _mdata[0].y;
	float	zmax = _mdata[0].z;
	float err, errnew;

	for (int i = 0; i < dataSize; i++)
	{
		if (xmax < _mdata[i].x)
			xmax = _mdata[i].x;
		if (ymax < _mdata[i].y)
			ymax = _mdata[i].y;
		if (zmax < _mdata[i].z)
			zmax = _mdata[i].z;
		if (xmin > _mdata[i].x)
			xmin = _mdata[i].x;
		if (ymin > _mdata[i].y)
			ymin = _mdata[i].y;
		if (zmin > _mdata[i].z)
			zmin = _mdata[i].z;
	}

	lastparam.xOffset = 0.5f * (xmax + xmin);
	lastparam.yOffset = 0.5f * (ymax + ymin);
	lastparam.zOffset = 0.5f * (zmax + zmin);
	lastparam.xScale = 0.5f * fabs(xmax - xmin);
	lastparam.yScale = 0.5f * fabs(ymax - ymin);
	lastparam.zScale = 0.5f * fabs(zmax - zmin);
	err = 0;
	for (int i = 0; i < dataSize; i++)
		err = err + fabs(powf((_mdata[i].x - lastparam.xOffset), 2) / powf(lastparam.xScale, 2) + powf((_mdata[i].y - lastparam.yOffset), 2) / powf(lastparam.yScale, 2) + powf((_mdata[i].z - lastparam.zOffset), 2) / powf(lastparam.zScale, 2) - 1);
	//printf("xc = %f yc = %f zc = %f, a = %f b = %f c= %f,初始化InitErr = %f\n", lastparam.xOffset, lastparam.yOffset, lastparam.zOffset, lastparam.xScale, lastparam.yScale, lastparam.zScale, err);
	int starttime = clock();
	for (int j = 0; j < 5000; j++)
	{
		param.xOffset = lastparam.xOffset + rand() / (RAND_MAX + 1.0f) - 0.5f;
		param.yOffset = lastparam.yOffset + rand() / (RAND_MAX + 1.0f) - 0.5f;
		param.zOffset = lastparam.zOffset + rand() / (RAND_MAX + 1.0f) - 0.5f;
		param.xScale = fabs(lastparam.xScale + rand() / (RAND_MAX + 1.0f) - 0.5f);
		param.yScale = fabs(lastparam.yScale + rand() / (RAND_MAX + 1.0f) - 0.5f);
		param.zScale = fabs(lastparam.zScale + rand() / (RAND_MAX + 1.0f) - 0.5f);
		errnew = 0;
		for (int i = 0; i < dataSize; i++)
			errnew = errnew + fabs(powf((_mdata[i].x - param.xOffset), 2) / powf(param.xScale, 2) + powf((_mdata[i].y - param.yOffset), 2) / powf(param.yScale, 2) + powf((_mdata[i].z - param.zOffset), 2) / powf(param.zScale, 2) - 1);
		if (errnew < err)
		{
			lastparam.xOffset = param.xOffset;
			lastparam.yOffset = param.yOffset;
			lastparam.zOffset = param.zOffset;
			lastparam.xScale = param.xScale;
			lastparam.yScale = param.yScale;
			lastparam.zScale = param.zScale;
			err = errnew;
		}
	}
	param.xScale = 200 / param.xScale;
	param.yScale = 200 / param.yScale;
	param.zScale = 200 / param.zScale;
	int durtime = clock() - starttime;

	printf("%f, %f, %f, %f, %f, %f, %f\n cost:%dms\n", param.xOffset, param.yOffset, param.zOffset, param.xScale, param.yScale, param.zScale, err, durtime);

	return param;
}


// 检查是否有重复的磁力计数据
bool isDuplicateData(float x, float y, float z, int index) {

	if (index < 0 || index >= 4000) {
		return false;
	}
	// 如果x, y, z完全相同，认为是重复数据
	if (mdata[index].x == x && mdata[index].y == y && mdata[index].z == z) {
#if ENABLE_DEBUG_LOG
		WriteLog(Info, "MagCalibrate: %.6f,%.6f,%.6f ==> Mag Data Duplicate\n",x,y,z);
#endif
		return true;
	}

	return false;
}

MagCaliParam GetMagDataFormFile()
{
	int maxdatasize = 4000;
	std::ifstream fin("data.txt");
	float x, y, z;
	int i = 0;
	clearMagData();
#if ENABLE_DEBUG_LOG
	WriteLog(Info, "MagCalibrate: Get mag data form file\n");
#endif
	while (fin) {
		fin >> x >> y >> z;
		// 加入这行，避免最后一行输出两次
		fin.get(); 

		// 使用checkMagnetNorm函数检测是否为异常值 && 
		// 检查当前读取到的磁力计xyz是否和上一次完全相同
		if (checkMagnetNorm(x, y, z) || isDuplicateData(x, y, z, i - 1)) {
			// 如果是异常值，跳过当前数据
			continue;
		}
		WriteLog(Info, "MagCalibrate: Load Magnet Data: %f,%f,%f\n",x,y,z);
		mdata[i].x = x;
		mdata[i].y = y;
		mdata[i].z = z;
		i++;

		if (i == maxdatasize)
			break;
	}

	MagCaliParam p = CalculateMagCalibrationParams(mdata, i);

	return p;
}

// 使用memset清空mdata数组
void clearMagData() {
	std::memset(mdata, 0, sizeof(mdata));  // 将mdata的内存清零
}

// 检查磁力计的模是否正常
bool checkMagnetNorm(float x, float y, float z) {
	// 计算磁力计数据的模
	float norm = std::sqrt(x * x + y * y + z * z);

#if ENABLE_DEBUG_LOG
	if (norm > 2000.0f) {
		WriteLog(Info, "Magnet data norm > 2000.0f\n");
	}
	if (norm == 0.0f) {
		WriteLog(Info, "Magnet data norm = 0.0f\n");
	}
#endif
	// 如果模大于2000或者等于0，则视为异常
	return (norm > 2000.0f || norm == 0.0f);
}