#include "ellipsoid.h"
#include "stdio.h"
#include "string.h"
#include "math.h"




#define printf(a,fmt...) 





/*=================================计算用的私有函数=====================================*/

//取绝对值
static float Abs(float a)
{
	return a < 0 ? -a : a;
}

//交换两行元素位置
static void Row2_swop_Row1(float matrix[MATRIX_SIZE][MATRIX_SIZE+1],int row1, int row2)
{
	float tmp = 0;
	for (int column = 0; column < MATRIX_SIZE + 1; column++)
	{
		tmp = matrix[row1][column];
		matrix[row1][column] = matrix[row2][column];
		matrix[row2][column] = tmp;
	}
}

//用把row行的元素乘以一个系数k
static void k_muiltply_Row(float matrix[MATRIX_SIZE][MATRIX_SIZE+1],float k, int row)
{
	for (int column = 0; column < MATRIX_SIZE + 1; column++)
		matrix[row][column] *= k;
}

//用一个数乘以row1行加到row2行上去
static void Row2_add_kRow1(float matrix[MATRIX_SIZE][MATRIX_SIZE+1],float k, int row1, int row2)
{
	for (int column = 0; column < MATRIX_SIZE + 1; column++)
		matrix[row2][column] += k*matrix[row1][column];
}

//列主元，第k次消元之前，把k行到MATRIX_SIZE的所有行进行冒泡排出最大，排序的依据是k列的元素的大小
static void MoveBiggestElement_to_Top(float matrix[MATRIX_SIZE][MATRIX_SIZE+1],int k)
{
	int row = 0, column = 0;

	for (row = k + 1; row < MATRIX_SIZE; row++)
	{
		if (Abs(matrix[k][k]) < Abs(matrix[row][k]))
		{
			Row2_swop_Row1(matrix,k, row);
		}
	}
}

//高斯消元法，求行阶梯型矩阵,返回0成功
static int Matrix_GaussElimination(float matrix[MATRIX_SIZE][MATRIX_SIZE+1])
{
	float k = 0;
	for (int cnt = 0; cnt < MATRIX_SIZE; cnt++)//进行第k次的运算，主要是针对k行以下的行数把k列的元素都变成0
	{
		//把k行依据k列的元素大小，进行排序
		MoveBiggestElement_to_Top(matrix,cnt);
		if (matrix[cnt][cnt] == 0)
			return(1);//返回值表示错误
		//把k行下面的行元素全部消成0，整行变化
		for (int row = cnt + 1; row < MATRIX_SIZE; row++)
		{
			k = -matrix[row][cnt] / matrix[cnt][cnt];
			Row2_add_kRow1(matrix,k, cnt, row);
		}
//		DispMatrix();
	}
	return 0;
}

//求行最简型矩阵，即把对角线的元素全部化成1
static void Matrix_RowSimplify(float matrix[MATRIX_SIZE][MATRIX_SIZE+1])
{
	float k = 0;
	for (int row = 0; row < MATRIX_SIZE; row++)
	{
		k = 1 / matrix[row][row];
		k_muiltply_Row(matrix,k, row);
	}
//	DispMatrix();
}

//求非齐次线性方程组的解
static void Matrix_Solve(float matrix[MATRIX_SIZE][MATRIX_SIZE+1],float* solve)
{
	for (short row = MATRIX_SIZE - 1; row >= 0; row--)
	{
		solve[row] = matrix[row][MATRIX_SIZE];
		for (int column = MATRIX_SIZE - 1; column >= row + 1; column--)
			solve[row] -= matrix[row][column] * solve[column];
	}
	printf("  a = %f| b = %f| c = %f| d = %f| e = %f| f = %f ", solve[0], solve[1], solve[2], solve[3], solve[4], solve[5]);
	printf("\r\n");
	printf("\r\n");
}

/*=================================计算用的私有函数End=====================================*/







//清零
void ellipsoid_reset(ellipsoid_struct *ell)
{
	if(ell==0) return;
	memset(ell,0,sizeof(ellipsoid_struct));
}




//添加点并求和
void ellipsoid_add_point(ellipsoid_struct *ell,float x,float y,float z)
{
	if(ell==0) return;
	
	
	ell->num++;
	double V[MATRIX_SIZE + 1];
	V[0] = y*y;
	V[1] = z*z;
	V[2] = x;
	V[3] = y;
	V[4] = z;
	V[5] = 1.0;
	V[6] = -x*x;
	
	//构建系数矩阵，并进行累加
	for (int row = 0; row < MATRIX_SIZE; row++)
	{
		for (int column = 0; column < MATRIX_SIZE + 1; column++)
		{
			ell->sum[row][column] += V[row] * V[column];
		}
	}
	//b向量是m_matrix[row][6]
	
}


//求平均值
void ellipsoid_average(ellipsoid_struct *ell)
{
	if(ell==0) return;

	for (int row = 0; row < MATRIX_SIZE; row++)
	for (int column = 0; column < MATRIX_SIZE + 1; column++)
		ell->avr[row][column]=ell->sum[row][column] / ell->num;
	
	//b向量是m_matrix[row][6]
}


//开始拟合
void ellipsoid_fitting(ellipsoid_struct *ell)
{
	if(ell==0) return;
	
	float solve[6]={0};

	//在添加一定数量的点之后求取平均值
	ellipsoid_average(ell);
	
	//求矩阵
	if(Matrix_GaussElimination(ell->avr)==0)
	{
		Matrix_RowSimplify(ell->avr);
		Matrix_Solve(ell->avr,solve);
		
		float a = 0, b = 0, c = 0, d = 0, e = 0, f = 0;
		a = solve[0];
		b = solve[1];
		c = solve[2];
		d = solve[3];
		e = solve[4];
		f = solve[5];

		ell->x = -c / 2;
		ell->y = -d / (2 * a);
		ell->z = -e / (2 * b);
		ell->a = sqrt(ell->x*ell->x + a*ell->y*ell->y + b*ell->z*ell->z - f);
		ell->b = ell->a / sqrtf(a);
		ell->c = ell->a / sqrtf(b);
		printf("  ((x - x0) / A) ^ 2 + ((y - y0) / B) ^ 2 + ((z - z0) / C) ^ 2 = 1 Ellipsoid result as below：\r\n");
		printf("\r\n");
		printf("  X0 = %f| Y0 = %f| Z0 = %f| A = %f| B = %f| C = %f \r\n", ell->x, ell->y,ell->z, ell->a, ell->b, ell->c);
		
	}
	
}