using ArduinoBluetoothAPI;
using System;
using System.Collections.Generic;
using System.Linq;
using UnityEngine;

public class o09DOF 
{
    static Vector3 AccIdentity = new Vector3(0, -1, 0);
    static Vector3 MagIdentity = new Vector3(-1, 2, 0).normalized;
    public class State
    {
        public long TimeGap;
        public Vector3 Acc = AccIdentity;
        public Vector3 Gyr;
        public Vector3 Mag = MagIdentity;
        public Quaternion Qua;
        public float Variance = 1;
    }

    List<State> States = new List<State>();

    Vector3 AccOld;
    Vector3 GyrOld;
    Vector3 MagOld;
    long TimeGapOld;
    /////////////////////g       degree/ms         
    public Quaternion o06DOFUpdate(Vector3 AccOld, Vector3 GyrOld, Vector3 MagOld, long TimeGapOld)
    {
        //Debug.Log(TimeGapOld);
        var Acc = this.AccOld;
        var Gyr = this.GyrOld;
        var Mag = this.MagOld;
        float TimeGap = (TimeGapOld + this.TimeGapOld) / 2;
        this.AccOld = AccOld;
        this.GyrOld = GyrOld;
        this.MagOld = MagOld;
        this.TimeGapOld = TimeGapOld;
        if (this.TimeGapOld <= 0)
            return Quaternion.identity;
        var Last = States.LastOrDefault() ?? new State();
        States.Add(new State());
        if (States.Count > 10)
            States.RemoveAt(0);
        var state = States.Last();
        state.Acc = Acc;
        state.Gyr = Gyr;
        state.Mag = Mag;
        //Debug.Log(TimeGap);

        /*
        var Accwit = GameObject.Find("Accwit");
        var Gyrwit = GameObject.Find("Gyrwit");
        var Magwit = GameObject.Find("Magwit");/**/
        var LastQuaternion = Last.Qua;
        //var LastQuaternion = Gyrwit.transform.localRotation;

        var newQua = new Quaternion();
        newQua.eulerAngles = Gyr * TimeGap;
        var quaGyr = LastQuaternion * newQua;



        float AccLengthToAngle = 90;//1倍引力差相当于多少度方差
        float MagLengthToAngle = 90;//1倍磁力差相当于多少度方差


        /*
         * 
        float GyrVariance = state.Variance + TimeGap/200 + Mathf.Pow((Gyr * TimeGap).magnitude * 0.03f,2);
        float AccVariance = TimeGap / 30 + Mathf.Pow((Acc.magnitude - 9.8f) / 9.8f * AccLengthToAngle, 2)+ Mathf.Pow(Vector3.Angle(Acc,Last.Acc) * 0.5f, 2);
        //Debug.Log(Mag.magnitude);
        float MagVariance = TimeGap / 100 + Mathf.Pow((Mag.magnitude - 500) / 500 * MagLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Mag, Last.Mag) * 0.1f, 2);
        /**/

        float GyrVariance = state.Variance + TimeGap / 100 + (Gyr * TimeGap).magnitude * 0.05f;
        float AccVariance = TimeGap / 30 + Mathf.Sqrt(Mathf.Pow((Acc.magnitude - 9.8f) / 9.8f * AccLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Acc, Last.Acc) * 0.5f, 2));
        Debug.Log(AccVariance);
        float MagVariance = TimeGap / 100 + Mathf.Sqrt(Mathf.Pow((Mag.magnitude - 500) / 500 * MagLengthToAngle, 2) + Mathf.Pow(Vector3.Angle(Mag, Last.Mag) * 0.1f, 2));
        state.Variance = state.Variance * AccVariance / (state.Variance + AccVariance);
        state.Variance = state.Variance * MagVariance / (state.Variance + MagVariance);

        var quaAccMag = o0Project.o0.FormQuaternion(AccIdentity, MagIdentity, Acc, Mag, AccVariance / (AccVariance + MagVariance));

        var quaMinRate = GyrVariance / (GyrVariance + Mathf.Max(AccVariance, MagVariance));
        var quaMaxRate = GyrVariance / (GyrVariance + Mathf.Min(AccVariance, MagVariance));
        Quaternion quaFirst = Quaternion.Slerp(quaGyr, quaAccMag, quaMinRate).normalized;

        float quaSecondRate = (quaMaxRate - quaMinRate) / (1 - quaMinRate);
        state.Qua = AccVariance < MagVariance ? o0Project.o0.FormQuaternion(quaFirst, AccIdentity, Acc, quaSecondRate) : o0Project.o0.FormQuaternion(quaFirst, MagIdentity, Mag, quaSecondRate);


        return state.Qua;
        //Image1.DrawLine();
    }

    public void Calibrate() 
    {
        AccIdentity = AccOld;
        MagIdentity = MagOld;
    }
}