using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using MathNet.Numerics;
using MathNet.Numerics.LinearAlgebra;
using MathNet.Numerics.LinearAlgebra.Double;
using o0.Geometry;
using o0.Num;
using UnityEngine;

namespace o0.IMU
{
    public class EllipsoidFitting//默认在无磁干扰环境下，有磁干扰则无法保证效果
    {
        static public EllipsoidFitting Fit(IEnumerable<Geometry.Vector<double>> Vector)
        {
            try
            {
                return new EllipsoidFitting(Vector);
            }
            catch (NonConvergenceException)
            {
                return null;
            }
        }



        [Serialize]
        public Geometry.Vector<double> Center { get; protected set; }
        public Matrix<double> CorrectMatrix { get; protected set; }
        [Serialize]
        public double[][] CorrectMatrixArray { get => CorrectMatrix.ToRowArrays(); set => CorrectMatrix = DenseMatrix.OfRowArrays(value); }
        [Serialize]
        public Geometry.Vector<double> Radius { get; protected set; }
        public EllipsoidFitting(Geometry.Vector<double> Center, Matrix<double> CorrectMatrix, Geometry.Vector<double> Radius)
        {
            this.Center = Center;
            this.CorrectMatrix = CorrectMatrix;
            this.Radius = Radius;
        }
        public EllipsoidFitting(IEnumerable<Geometry.Vector<double>> Vector)//无法拟合时会丢NonConvergenceException
        {
            int mag_data_counter = Vector.Count();    //mag数据数量
            double mag_x, mag_y, mag_z;
            var mat_D = CreateMatrix.Dense<double>(mag_data_counter, 9);

            //读取mag
            for (int i = 0; i < mag_data_counter; i++)
            {
                //mag_x_y_z赋值
                var Vi = Vector.ElementAt(i);
                mag_x = Vi.x;
                mag_y = Vi.y;
                mag_z = Vi.z;

                mat_D[i, 0] = mag_x * mag_x;
                mat_D[i, 1] = mag_y * mag_y;
                mat_D[i, 2] = mag_z * mag_z;
                mat_D[i, 3] = 2 * mag_x * mag_y;
                mat_D[i, 4] = 2 * mag_x * mag_z;
                mat_D[i, 5] = 2 * mag_y * mag_z;
                mat_D[i, 6] = 2 * mag_x;
                mat_D[i, 7] = 2 * mag_y;
                mat_D[i, 8] = 2 * mag_z;

            }

            var mat_DT = mat_D.Transpose();
            var mat_Ones = CreateMatrix.Dense<double>(mag_data_counter, 1, 1.0);
            var mat_Result = (mat_DT * mat_D).Inverse() * (mat_DT * mat_Ones);
            var mat_A_4x4 = CreateMatrix.Dense<double>(4, 4);

            mat_A_4x4[0, 0] = mat_Result[0, 0];
            mat_A_4x4[0, 1] = mat_Result[3, 0];
            mat_A_4x4[0, 2] = mat_Result[4, 0];
            mat_A_4x4[0, 3] = mat_Result[6, 0];

            mat_A_4x4[1, 0] = mat_Result[3, 0];
            mat_A_4x4[1, 1] = mat_Result[1, 0];
            mat_A_4x4[1, 2] = mat_Result[5, 0];
            mat_A_4x4[1, 3] = mat_Result[7, 0];

            mat_A_4x4[2, 0] = mat_Result[4, 0];
            mat_A_4x4[2, 1] = mat_Result[5, 0];
            mat_A_4x4[2, 2] = mat_Result[2, 0];
            mat_A_4x4[2, 3] = mat_Result[8, 0];

            mat_A_4x4[3, 0] = mat_Result[6, 0];
            mat_A_4x4[3, 1] = mat_Result[7, 0];
            mat_A_4x4[3, 2] = mat_Result[8, 0];
            mat_A_4x4[3, 3] = -1.0;


            var mat_Center = -((mat_A_4x4.SubMatrix(0, 3, 0, 3)).Inverse() * mat_Result.SubMatrix(6, 3, 0, 1));
            //椭球圆心					//分块，从0,0开始的3*3的矩阵

            var mat_T_4x4 = CreateMatrix.DenseIdentity<double>(4, 4);
            mat_T_4x4.SetSubMatrix(3, 1, 0, 3, mat_Center.Transpose());

            var mat_R = mat_T_4x4 * mat_A_4x4 * mat_T_4x4.Transpose();

            var evd = mat_R.SubMatrix(0, 3, 0, 3) / -mat_R[3, 3];
            var eig = evd.Evd();

            var mat_Eigval = CreateVector.Dense<double>(3);
            var mat_Evecs = eig.EigenVectors;


            mat_Eigval[0] = eig.EigenValues[0].Real;   //特征值的实部
            mat_Eigval[1] = eig.EigenValues[1].Real;
            mat_Eigval[2] = eig.EigenValues[2].Real;


            var mat_Radii = mat_Eigval.Map(delegate (double x)
            {
                return 1.0 / Math.Sqrt(Math.Abs(x));
            });    //椭球半径，特征值倒数后开方

            var mat_Scale = CreateMatrix.DenseIdentity<double>(3, 3);
            mat_Scale[0, 0] = mat_Radii[0];
            mat_Scale[1, 1] = mat_Radii[1];
            mat_Scale[2, 2] = mat_Radii[2];

            //double min_Radii = mat_Radii.Minimum();           //返回最小的元素

            mat_Scale = mat_Scale.Inverse();// * min_Radii;
            var mat_Correct = mat_Evecs * mat_Scale * mat_Evecs.Transpose();

            //_Center = new Vector3((float)mat_Center[0], (float)mat_Center[1], (float)mat_Center[2]);

            /*
            Debug.Log("The Ellipsoid center is:" + mat_Center.ToString());
            Debug.Log("The Ellipsoid radii is:" + mat_Radii.ToString());
            Debug.Log("The scale matrix is:" + mat_Scale.ToString());
            Debug.Log("The correct matrix is:" + mat_Correct.ToString());
            /**/

            Center = new Geometry.Vector<double>(mat_Center[0, 0], mat_Center[1, 0], mat_Center[2, 0]);
            CorrectMatrix = mat_Correct;
            Radius = new Geometry.Vector<double>(mat_Radii[0], mat_Radii[1], mat_Radii[2]);
        }

        //原始向量映射到椭球拟合后的空间
        public Geometry.Vector<double> Map(Geometry.Vector<double> Vector)
        {
            /*
            if (CorrectMatrix == null)
                throw new Exception("o0 EllipsoidFitting CorrectMatrix Is Null");/**/

            Vector -= Center;

            var MathNetV = CreateVector.Dense<double>(3);
            MathNetV[0] = Vector.x;
            MathNetV[1] = Vector.y;
            MathNetV[2] = Vector.z;
            //MathNetV = (MathNetV * mat_Scale) * mat_Correct;
            MathNetV = MathNetV * CorrectMatrix;

            Vector = new Geometry.Vector<double>(MathNetV[0], MathNetV[1], MathNetV[2]);
            //Debug.Log(v.magnitude);
            return Vector;
        }
    }
    
}
