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

namespace ZIM
{
    // 固定时间间隔下，预测位置轨迹
    public class SimpleLocationEstimation
    {
        //public bool Initialized;
        public Vector2 Predict;

        Vector2 LastLocation;
        Vector2 LastSpeed;
        Vector2 LastAcc;
        float AccChangeScale;
        List<Vector2> Locations;

        // 卡尔曼滤波作用于速度
        UKF filterx;
        UKF filtery;

        public SimpleLocationEstimation(float accChangeScale = 1)
        {
            Locations = new List<Vector2>();
            AccChangeScale = accChangeScale;
            filterx = new UKF();
            filtery = new UKF();
        }

        public Vector2 Update(Vector2 loc)
        {
            if (Locations == null)
            {
                var newSpeed = loc - LastLocation;
                var newAcc = newSpeed - LastSpeed;
                var accPredict = newAcc + (newAcc - LastAcc) * AccChangeScale;
                var speedPredict = newSpeed + accPredict;
                //if (speedPredict.x * newSpeed.x < 0)
                //    speedPredict.x = 0;
                //if (speedPredict.y * newSpeed.y < 0)
                //    speedPredict.y = 0;
                //var LocationPredict = loc + speedPredict;

                filterx.Update(new double[] { speedPredict.x });
                filtery.Update(new double[] { speedPredict.y });

                var LocationPredict = loc + new Vector2((float)filterx.getState()[0], (float)filtery.getState()[0]);

                LastSpeed = newSpeed;
                LastAcc = newAcc;
                LastLocation = loc;
                return Predict = LocationPredict;
                //return Predict = new Vector2((float)filterx.getState()[0], (float)filtery.getState()[0]);
            }
            if (Locations.Count < 2)
            {
                Locations.Add(loc);
            }
            else
            {
                Locations.Add(loc);
                var speed = new Vector2[] { Locations.ElementAt(1) - Locations.ElementAt(0), Locations.ElementAt(2) - Locations.ElementAt(1) };
                LastLocation = loc;
                LastSpeed = speed[1];
                LastAcc = speed[1] - speed[0];
                Locations = null;
            }
            return Predict = loc;
        }

    }
}