using System;
using Aitex.Core.RT.DataCenter;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.SCCore;

namespace MECF.Framework.RT.EquipmentLibrary.HardwareUnits.Temps.Filters
{
    public class KalmanFilter : FilterBase
    {
        private const double INIT_TEMP = 600.0;

        private bool _isInitialized = false;

        private double[,] F;
        private double[,] H;
        private double[,] Q;
        private double[,] P;
        private double[] x;

        public KalmanFilter(string module, string filterName, string scBasePath, double pollIntervalMs) 
            : base(module, filterName, scBasePath)
        {
            Dt = pollIntervalMs / 1000.0;
        }

        public double Dt { get; }

        public double R { get; private set; }

        public double QScale { get; private set; }

        public (double temp, double rate) LastTempRate { get; private set; }

        public override bool Initialize(out string reason)
        {
            reason = "";

            DATA.Subscribe($"{FilterName}.R", () => R);
            DATA.Subscribe($"{FilterName}.QScale", () => QScale);
            DATA.Subscribe($"{FilterName}.DeltaT", () => Dt);

            var scName = $"{ScBase}.R";
            R = SC.SafeGetValue(scName, 0d);
            SC.RegisterValueChangedCallback(scName, value =>
            {
                if (value is not double rValue)
                    LOG.Warning($"{scName} Invalid value for Kalman R: '{value}', expected an double value.");
                else if (R != rValue)
                {
                    R = rValue;
                    Reset();
                }
            });

            scName = $"{ScBase}.QScale";
            QScale = SC.SafeGetValue(scName, 0d);
            SC.RegisterValueChangedCallback(scName, value =>
            {
                if (value is not double qValue)
                    LOG.Warning($"{scName} Invalid value for Kalman QScale: '{value}', expected an double value.");
                else if(QScale != qValue)
                {
                    QScale = qValue;
                    Reset();
                }
            });

            // 移除对Dt的重新获取，使用构造函数传入的值
            // 如果需要从SC获取Dt，可以取消注释下面的代码
            /*
            scName = $"TempSensors.PollInterval";
            Dt = SC.GetValue(scName, 1000) / 1000.0; // Convert milliseconds to seconds
            SC.RegisterValueChangedCallback(scName, value =>
            {
                if (value is double dtValue && Dt != dtValue)
                {
                    Dt = dtValue / 1000; // Convert milliseconds to seconds
                    Reset();
                }
                else
                    LOG.Warning($"{FilterName} Invalid value for Kalman Dt: {value}. Expected an double.");
            });
            */

            Reset();

            return true;
        }

        public void InitializeForUnitTest(double r, double qScale)
        {
            R = r;
            QScale = qScale;
            Reset();
        }

        protected override double HandleFilter(double lastTemp)
        {
            if (!_isInitialized)
            {
                x[0] = lastTemp;
                x[1] = 0;
                _isInitialized = true;
                LastTempRate = (x[0], x[1]);
                return LastTempRate.temp;
            }

            // ---------- 预测 ----------
            var xPred = MatMul(F, x);
            var FP = MatMul(F, P);
            var FPFt = MatMul(FP, F, transposeA: false, transposeB: true);
            var PPred = Add(FPFt, Q);

            // ---------- 更新 ----------
            double[] y = [lastTemp - Dot(H, xPred)];  // 残差
            // 修正S和K的计算过程
            var PPred_Ht = MatMul(PPred, [H[0, 0], H[0, 1]]);
            var S = H[0, 0] * PPred_Ht[0] + H[0, 1] * PPred_Ht[1] + R;
            var K = new[] { PPred_Ht[0] / S, PPred_Ht[1] / S };

            // 状态更新
            x[0] = xPred[0] + K[0] * y[0];
            x[1] = xPred[1] + K[1] * y[0];

            // 协方差更新
            var KH = new[,]
            {
                { K[0] * H[0,0], K[0] * H[0,1] },
                { K[1] * H[0,0], K[1] * H[0,1] }
            };
            var I_KH = Subtract(Identity(2), KH);
            P = MatMul(I_KH, PPred);

            LastTempRate = (x[0], x[1]);
            return LastTempRate.temp;
        }

        public override void Reset()
        {
            base.Reset();

            // 状态转移矩阵
            F = new[,]
            {
                { 1, Dt },
                { 0, 1 }
            };

            // 观测矩阵
            H = new double[,] { { 1, 0 } };

            // 过程噪声协方差
            Q = new[,]
            {
                { QScale * Math.Pow(Dt, 4) / 4, QScale * Math.Pow(Dt, 3) / 2 },
                { QScale * Math.Pow(Dt, 3) / 2, QScale * Math.Pow(Dt, 2) }
            };

            P = new double[,]
            {
                { 1, 0 },
                { 0, 1 }
            };

            x = [0, 0];

            //x[0] = INIT_TEMP;
            //x[1] = 0;
            _isInitialized = false;

        }

        #region Util Methods

        private static double[] MatMul(double[,] A, double[] x)
        {
            return
            [
                A[0, 0] * x[0] + A[0, 1] * x[1],
                A[1, 0] * x[0] + A[1, 1] * x[1]
            ];
        }

        private static double[,] MatMul(double[,] A, double[,] B, bool transposeA = false, bool transposeB = false)
        {
            var rows = transposeA ? A.GetLength(1) : A.GetLength(0);
            var cols = transposeB ? B.GetLength(0) : B.GetLength(1);
            var shared = transposeA ? A.GetLength(0) : A.GetLength(1);
            var result = new double[rows, cols];

            for (var i = 0; i < rows; i++)
            for (var j = 0; j < cols; j++)
            for (var k = 0; k < shared; k++)
                result[i, j] +=
                    (transposeA ? A[k, i] : A[i, k]) *
                    (transposeB ? B[j, k] : B[k, j]);
            return result;
        }

        private static double Dot(double[,] H, double[] x)
        {
            return H[0, 0] * x[0] + H[0, 1] * x[1];
        }

        private static double[,] Add(double[,] A, double[,] B)
        {
            int rows = A.GetLength(0), cols = A.GetLength(1);
            var result = new double[rows, cols];
            for (var i = 0; i < rows; i++)
            for (var j = 0; j < cols; j++)
                result[i, j] = A[i, j] + B[i, j];
            return result;
        }

        private static double[,] Subtract(double[,] A, double[,] B)
        {
            int rows = A.GetLength(0), cols = A.GetLength(1);
            var result = new double[rows, cols];
            for (var i = 0; i < rows; i++)
            for (var j = 0; j < cols; j++)
                result[i, j] = A[i, j] - B[i, j];
            return result;
        }

        private static double[,] Identity(int n)
        {
            var result = new double[n, n];
            for (var i = 0; i < n; i++)
                result[i, i] = 1;
            return result;
        }

        #endregion
    }
}
