/******************** (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>

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.5 * (xmax + xmin);
	lastparam.yOffset = 0.5 * (ymax + ymin);
	lastparam.zOffset = 0.5 * (zmax + zmin);
	lastparam.xScale = 0.5 * abs(xmax - xmin);
	lastparam.yScale = 0.5 * abs(ymax - ymin);
	lastparam.zScale = 0.5 * abs(zmax - zmin);
	err = 0;
	for (int i = 0; i < dataSize; i++)
		err = err + fabs(pow((_mdata[i].x - lastparam.xOffset), 2) / pow(lastparam.xScale, 2) + pow((_mdata[i].y - lastparam.yOffset), 2) / pow(lastparam.yScale, 2) + pow((_mdata[i].z - lastparam.zOffset), 2) / pow(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.0) - 0.5;
		param.yOffset = lastparam.yOffset + rand() / (RAND_MAX + 1.0) - 0.5;
		param.zOffset = lastparam.zOffset + rand() / (RAND_MAX + 1.0) - 0.5;
		param.xScale = fabs(lastparam.xScale + rand() / (RAND_MAX + 1.0) - 0.5);
		param.yScale = fabs(lastparam.yScale + rand() / (RAND_MAX + 1.0) - 0.5);
		param.zScale = fabs(lastparam.zScale + rand() / (RAND_MAX + 1.0) - 0.5);
		errnew = 0;
		for (int i = 0; i < dataSize; i++)
			errnew = errnew + fabs(pow((_mdata[i].x - param.xOffset), 2) / pow(param.xScale, 2) + pow((_mdata[i].y - param.yOffset), 2) / pow(param.yScale, 2) + pow((_mdata[i].z - param.zOffset), 2) / pow(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;
}
MagCaliParam GetMagDataFormFile()
{
	int maxdatasize = 4000;
	std::ifstream fin("data.txt");
	float x, y, z, x1, y1, z1;
	int i = 0;
	MagData mdata[4000];

	while (fin) {
		fin >> x >> y >> z;
		fin.get(); // 加入这行，避免最后一行输出两次
		mdata[i].x = x;
		mdata[i].y = y;
		mdata[i].z = z;

		i++;

		if (i == maxdatasize)
			break;
	}

	MagCaliParam p = CalculateMagCalibrationParams(mdata, i);

	return p;
}