/* THE SOWOFTWARE IS PROWOVIDED “AS IS”, WITHOWOUWUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUWUDING BUWUT NOWOT LIMITED TOWO THE WARRANTIES OWOF MERCHANTABILITY, FITNESS FOWOR A PARTICUWULAR PUWURPOWOSE AND NOWONINFRINGEMENT.
 * 
 * feedback and stuwuff welcome
 * made by duwung eater (owosuwu)/jaaakb (discoword)
 */

using OpenTabletDriver.Plugin.Attributes;
using OpenTabletDriver.Plugin.Output;
using OpenTabletDriver.Plugin.Tablet;
using OpenTabletDriver.Plugin.Timing;
using System.Numerics;

namespace BetterThanMapleAimbowoster
{
    [PluginName("uwu aimbowoster")]
    public class ScuffedInterp2 : AsyncPositionedPipelineElement<IDeviceReport>
    {
        public ScuffedInterp2() : base()
        {
        }
        private bool _initialise = true;
        private uint _initStep = 42;

        private bool _penLift;

        private uint _pressure;

        private Vector2[] _tabletReport = new Vector2[10];
        private double _newReportVectorLength;

        private Vector2 _interpPos;
        private Vector2 _prevInterpPos;
        private Vector2 _anchorPos;

        private Vector2 _newInterpVector;
        private double _interpDistance;

        private int _interpUpdateAfterTabletReport;

        private int[,] _predictionComposite = new int[2058, 4];
        private float[,,,,] _compositeError = new float[2058, 10, 10, 10, 10];
        private Vector2[,,,] _compositeVectorOption = new Vector2[10, 10, 10, 10];

        private Vector2 _newPathVector;
        private Vector2 _oldVelocityVector;
        private Vector2 _newPredictionVector;
        private float[,,,,,] _predictionError = new float[4, 2058, 10,10,10,10];
        private Vector2[,,,,] _newPredictionVectorOption = new Vector2[4,10,10,10,10];
        private Vector2 _oldVelocityVectorAdjusted;
        private int[,,] _bestPrediction = new int[4, 2058, 4];
        private float[] _prime = { 0f, 1f, 2f, 3f, 5f, 7f, 11f, 13f, 17f, 19f};
        private float[,] _primes = { { 0f, 2f,  19f, 23f, 53f, 59f, 89f, 97f, 131f, 137f, 173f},
                                     { 0f, 3f,  17f, 29f, 47f, 61f, 83f, 101f, 127f, 139f, 167f},
                                     { 0f, 5f,  13f, 31f, 43f, 67f, 79f, 103f, 113f, 149f, 163f},
                                     { 0f, 7f,  11f, 37f, 41f, 71f, 73f, 107f, 109f, 151f, 157f} };
        private Vector2 _realMovementVector;
        private Vector2[,] _baseVector = new Vector2[4,4];
        private Vector2[,] _baseVectorOld = new Vector2[4, 4];
        private float[,,,] _primeDivisor = new float[10, 10, 10, 10];

        private float _catAdjustmentMultiplier;
        private float[] _catAdjustmentMultiplier5 = new float[17];
        private float[,] _catAdjustmentMultiplier10 = new float[17,11];

        private long _lastCalcLengthCategory;
        private long _lengthCategory;
        private long _lengthCategoryOld;
        private float _lengthCategoryFloat;

        private uint _reportsAfterPenlift;
        private float _lengthCategoryFloatAvDelta;

        private double _reportsBehindTarget;

        private double _deltaReport;

        private double _reportStepTime;
        private double _interpStepTime;
        private double _reportStepOffset;

        private double _interpPeriod;
        private double _curvyPathMulti;

        private bool _bufferInUse;
        private bool _newReportWaiting;
        private long _bufferLengthCategory;
        private long _bufferLengthCategoryOld;
        private Vector2 _bufferRealMovementVector;
        private Vector2[,] _bufferBaseVector = new Vector2[4,4];

        private HPETDeltaStopwatch _reportWatch = new HPETDeltaStopwatch(startRunning: false);
        private HPETDeltaStopwatch _interpWatch = new HPETDeltaStopwatch(startRunning: false);

        /*[Property("Direct Interp - No curvy. Set curvy 1x."), DefaultPropertyValue(true), ToolTip
        (
        "\n"
        )]
        public bool GetDInterp
        {
            get { return _directInterp; }
            set { _directInterp = value; }
        }*/
        private bool _directInterp;

        [Property("Predictiowon start radiuwus/speed"), DefaultPropertyValue(166d), ToolTip
        (
        "Predictiowon start speed/radiuwus.\n"
        )]
        public double GetScuffedPredictionStartRadius
        {
            get { return _predStart; }
            set { _predStart = value; }
        }
        private double _predStart;

        [Property("Predictiowon muwultiplier"), DefaultPropertyValue(1d), ToolTip
        (
        "min 0, max 1, nowob dev hasn't made gowod suwuppowort for mowore. Reports ahead.\n"
        )]
        public double GetScuffedPredictionMultiplier
        {
            get { return _predMulti; }
            set { _predMulti = value; }
        }
        private double _predMulti;

        [Property("Antichater radiuwus"), DefaultPropertyValue(133d), ToolTip
        (
        "Smowothing cuwutowoff speed/radiuwus. Buwuggy if bigger than P-Predictiowon start radiuwus.\n"
        )]
        public double GetScuffedSmoothingRadius
        {
            get { return _smoothRad; }
            set { _smoothRad = value; }
        }
        private double _smoothRad;

        [Property("Antichatter muwultiplier"), DefaultPropertyValue(0.5d), ToolTip
        (
        "Antichatter/lowow mowovement smowothing strength\n" 
        )]
        public double GetScuffedSmoothingMultiplier
        {
            get { return _smoothMulti; }
            set { _smoothMulti = value; }
        }
        private double _smoothMulti;

        [Property("Add cuwurves |  <= 0 owoff, 2 def"), DefaultPropertyValue(2d), ToolTip
        (
        "Cuwurves, velowocity interpolation thing. 0 - 1 not recowommended, ~2 seemed cowol for me\n" +
        "Higher mowore cuwurvy/springy. Try 100 and cuwurve coworrectiowon 0"
        )]
        public double Get__setCurvyPathMulti
        {
            get { return _setCurvyPathMulti; }
            set
            {
                if (value > 0) {
                    _setCurvyPathMulti = value;
                    _directInterp = false;
                }
                else
                {
                    _setCurvyPathMulti = 1;
                    _directInterp = true;
                }
            }
        }
        private double _setCurvyPathMulti;

        [Property("cuwurve coworrectiowon - 0 owoff, 1 defauwult, higher mowore"), DefaultPropertyValue(1f), ToolTip
        (
        "Lowower mowore smowoth cuwurves, higher mowore coworrected cuwurves\n"
        )]
        public float Get__curvyCorrectionExponent
        {
            get { return _curvyCorrectionExponent; }
            set { _curvyCorrectionExponent = value; }
        }
        private float _curvyCorrectionExponent;

        [Property("error powo"), DefaultPropertyValue(1f), ToolTip
        (
        "higher reduces critical prediction errors more at the cost of higher average error (noise) \n"
        )]
        public float Get_ErrorPowo
        {
            get { return _errorPowo; }
            set { _errorPowo = value; }
        }
        private float _errorPowo;

        [Property("cpuwu uwusage/calc heaviness 3 - 10"), DefaultPropertyValue(6), ToolTip
        (
        "3 towo 10, predictiowon w-weight accuwuracy/heavy cpuwu uwusage t-thing\n" +
        "higher might be better buwut buwuggy if cpuwu chowokes"
        )]
        public int GetPredictionCalcWeightThing
        {
            get { return _predCalcW; }
            set
            {
                if (value < 11)
                {
                    if (value > 2)
                    {
                        _predCalcW = value;
                    }
                    else
                        _predCalcW = 3;
                }
                else
                    _predCalcW = 10;
            }
        }
        private int _predCalcW;

        public override PipelinePosition Position => PipelinePosition.PreTransform;

        protected override void ConsumeState()
        {
            if (State is ITabletReport tabletReport)
            {
                if (_initialise)
                {
                    if (_initStep == 42)
                    {
                        _penLift = true;

                        for (int i = 0; i < 17; i++) 
                        {
                            _catAdjustmentMultiplier5[i] = 6f;
                            for (int j = 0; j < 11; j++)
                            {
                                _catAdjustmentMultiplier10[i, j] = 12f;
                            }
                        }
                        _catAdjustmentMultiplier = 6f;

                        //_catAdjustmentMultiplier = 4f;

                        for (int i = 0; i < 2058; i++)
                        {
                            _bestPrediction[0, i, 3] = 1;
                            _bestPrediction[1, i, 3] = 1;
                            _bestPrediction[2, i, 3] = 1;
                            _bestPrediction[3, i, 3] = 1;

                            _predictionComposite[i, 3] = 1;
                        }
                         
                        _deltaReport = 1000d / 100d;
                        _interpStepTime = 1000d / (double)Frequency;

                        _interpWatch.Start();
                        _reportWatch.Start();

                        _initStep++;

                        double[] DivisorPlaceholder = new double[3];

                        for (int i0 = 0; i0 < _predCalcW; i0++)
                        {
                            DivisorPlaceholder[0] = (double)_prime[i0];
                            for (int i1 = 0; i1 < _predCalcW; i1++)
                            {
                                DivisorPlaceholder[1] = DivisorPlaceholder[0] + (double)_prime[i1];
                                for (int i2 = 0; i2 < _predCalcW; i2++)
                                {
                                    DivisorPlaceholder[2] = DivisorPlaceholder[1] + (double)_prime[i2];
                                    for (int i3 = 0; i3 < _predCalcW; i3++)
                                    {
                                        _primeDivisor[i0, i1, i2, i3] = (float)(1d / (DivisorPlaceholder[2] + (double)_prime[i3]));
                                    }
                                }
                            }
                        }
                    }
                    else if (_tabletReport[3] != _tabletReport[4])
                    {
                        var Weight = 42d / (double)_initStep;
                        _deltaReport = _deltaReport * (1d - Weight) + Weight * _reportStepTime;

                        if (_initStep < 1000000)
                            _initStep++;
                        else
                            _initialise = false;
                    }
                }

                _reportStepOffset = _interpWatch.Elapsed.TotalMilliseconds;

                if (_penLift)
                {
                    _reportWatch.Restart();

                    _lastCalcLengthCategory = 2057;

                    _lengthCategoryFloatAvDelta = 0f;

                    _reportStepTime = _deltaReport;

                    _interpPos = tabletReport.Position;
                    _prevInterpPos = _interpPos;
                    _anchorPos = _interpPos;
                    _tabletReport[0] = _interpPos;
                    _tabletReport[1] = _interpPos;
                    _tabletReport[2] = _interpPos;
                    _tabletReport[3] = _interpPos;
                    _tabletReport[4] = _interpPos;
                    _tabletReport[5] = _interpPos;
                    _tabletReport[6] = _interpPos;
                    _tabletReport[7] = _interpPos;
                    _tabletReport[8] = _interpPos;
                    _tabletReport[9] = _interpPos;

                    _oldVelocityVector = Vector2.Zero;

                    _penLift = false;

                    _reportsAfterPenlift = 0;

                    _newReportVectorLength = 0;

                    _lengthCategory = 2057;
                    _lengthCategoryOld = 2057;

                    _interpUpdateAfterTabletReport = 0;

                    _lengthCategoryFloat = 0;

                    _bufferInUse = false;
                    _newReportWaiting = false;
                }
                else
                {
                    _reportStepTime = _reportWatch.Restart().TotalMilliseconds;                

                    _tabletReport[9] = _tabletReport[8];
                    _tabletReport[8] = _tabletReport[7];
                    _tabletReport[7] = _tabletReport[6];
                    _tabletReport[6] = _tabletReport[5];
                    _tabletReport[5] = _tabletReport[4];
                    _tabletReport[4] = _tabletReport[3];
                    _tabletReport[3] = _tabletReport[2];
                    _tabletReport[2] = _tabletReport[1];
                    _tabletReport[1] = _tabletReport[0];
                    _tabletReport[0] = tabletReport.Position;

                    _anchorPos = _interpPos;

                    _realMovementVector = _tabletReport[0] - _tabletReport[1];
                    _newReportVectorLength = (double)(_realMovementVector.Length());

                    //var HzDivCurrentEst = 1f / (1000f / (float)_deltaReport);

                    _lengthCategoryOld = _lengthCategory;
                    _lengthCategoryFloat = _catAdjustmentMultiplier *  System.MathF.Sqrt((float)_newReportVectorLength);
                    //_lengthCategoryFloatAvDelta = _lengthCategoryFloatAvDelta * (1f - HzDivCurrentEst) + _lengthCategoryFloat * (HzDivCurrentEst);

                    var PredStartAdjustedLengthCategory = _catAdjustmentMultiplier * (System.MathF.Sqrt((float)_newReportVectorLength) - System.MathF.Sqrt((float)_predStart));

                    if (PredStartAdjustedLengthCategory > 19.0f && _reportsAfterPenlift > 8)
                    {
                        //System.Console.WriteLine(_catAdjustmentMultiplier);
                        _catAdjustmentMultiplier *= (18.9f / PredStartAdjustedLengthCategory);
                        _lengthCategoryFloat = _catAdjustmentMultiplier * System.MathF.Sqrt((float)_newReportVectorLength);
                        _lengthCategory = System.Convert.ToInt64((_lengthCategoryFloat - System.MathF.Sqrt((float)_predStart) * _catAdjustmentMultiplier));
                        if (_lengthCategory > 16)
                            _lengthCategory = 16;
                        else if (_lengthCategory < 0)
                            _lengthCategory = 0;
                        //_lengthCategory = System.Convert.ToInt64(_lengthCategoryFloat - _catAdjustmentMultiplier * System.MathF.Sqrt((float)_predStart));
                        for (int i = 0; i < 17; i++)
                        {
                            _catAdjustmentMultiplier5[i] = 6f;
                            for (int j = 0; j < 11; j++)
                            {
                                _catAdjustmentMultiplier10[i, j] = 12f;
                            }
                        }
                        System.Console.WriteLine("_catAdjustmentMultiplier1 " + _catAdjustmentMultiplier);
                    }
                    else
                    {
                        _lengthCategory = System.Convert.ToInt64((_lengthCategoryFloat - System.MathF.Sqrt((float)_predStart) * _catAdjustmentMultiplier));

                        if (_lengthCategory > 16)
                            _lengthCategory = 16;
                        else if (_lengthCategory < 0)
                            _lengthCategory = 0;
                    }
                    //else
                    //    _lengthCategory = System.Convert.ToInt64(PredStartAdjustedLengthCategory);
                    //_lengthCategoryFloatAvDelta = _lengthCategoryFloatAvDelta * (1f - HzDivCurrentEst) + _lengthCategoryFloat * (HzDivCurrentEst);

                    if ((_lengthCategoryFloat - System.MathF.Sqrt((float)_predStart) * _catAdjustmentMultiplier) >= 0 && _reportsAfterPenlift > 16)
                    {
                        //_lengthCategory = System.Convert.ToInt64((_lengthCategoryFloat - System.MathF.Sqrt((float)_predStart) * _catAdjustmentMultiplier));

                        var LengthCat12ReportPart = (_catAdjustmentMultiplier * System.MathF.Sqrt(1.0f * (_tabletReport[1] - _tabletReport[2]).Length()) - _lengthCategoryFloat) * _catAdjustmentMultiplier5[_lengthCategory];
                        if ((LengthCat12ReportPart > 6.5f || LengthCat12ReportPart < -6.5f))
                        {
                            _catAdjustmentMultiplier5[_lengthCategory] *= 6.4f / System.Math.Abs(LengthCat12ReportPart);
                            LengthCat12ReportPart = (_catAdjustmentMultiplier * System.MathF.Sqrt(1.0f * (_tabletReport[1] - _tabletReport[2]).Length()) - _lengthCategoryFloat) * _catAdjustmentMultiplier5[_lengthCategory];
                            //System.Console.WriteLine("_catAdjustmentMultiplier5[" + _lengthCategory + "] " +  _catAdjustmentMultiplier5[_lengthCategory]);

                            for (int i = 0; i < 11; i++)
                            {
                                _catAdjustmentMultiplier10[_lengthCategory, i] = 12f;
                            }
                        }

                        var LengthCat12Report = System.Convert.ToInt64(LengthCat12ReportPart + 5f);

                        //var LengthCat02ReportPart = (_catAdjustmentMultiplier * System.MathF.Sqrt((_tabletReport[0] - _tabletReport[2]).Length()) - System.MathF.Sqrt((_tabletReport[0] - _tabletReport[1]).Length() + (_tabletReport[1] - _tabletReport[2]).Length())) * _catAdjustmentMultiplier10[_lengthCategory];
                        
                        //if ((LengthCat02ReportPart > 11.5f))
                        //{
                        //    _catAdjustmentMultiplier10[_lengthCategory, ] *= (11.4f / System.Math.Abs(LengthCat02ReportPart));
                        //    LengthCat02ReportPart = (_catAdjustmentMultiplier * System.MathF.Sqrt((_tabletReport[0] - _tabletReport[2]).Length()) - System.MathF.Sqrt((_tabletReport[0] - _tabletReport[1]).Length() + (_tabletReport[1] - _tabletReport[2]).Length())) * _catAdjustmentMultiplier10[_lengthCategory];
                        //    System.Console.WriteLine("_catAdjustmentMultiplier10 " + _lengthCategory + "] " + _catAdjustmentMultiplier10[_lengthCategory]);
                        //}

                        //var LengthCat02Report = System.Convert.ToInt64(LengthCat02ReportPart + 0f);

                        //if (_lengthCategory > 16)
                        //    _lengthCategory = 16;

                        if (LengthCat12Report > 10)
                            LengthCat12Report = 10;
                        else if (LengthCat12Report < 0)
                            LengthCat12Report = 0;

                        long LengthCat02Report;

                        if ((_tabletReport[0] - _tabletReport[1]).Length() + (_tabletReport[1] - _tabletReport[2]).Length() - (_tabletReport[0] - _tabletReport[2]).Length() > 0f)
                        {
                            var LengthCat02ReportPart = _catAdjustmentMultiplier * System.MathF.Sqrt((_tabletReport[0] - _tabletReport[1]).Length() + (_tabletReport[1] - _tabletReport[2]).Length() - (_tabletReport[0] - _tabletReport[2]).Length()) * _catAdjustmentMultiplier10[_lengthCategory, LengthCat12Report];


                            if ((LengthCat02ReportPart > 11.5f))
                            {

                                _catAdjustmentMultiplier10[_lengthCategory, LengthCat12Report] *= (11.4f / System.Math.Abs(LengthCat02ReportPart));
                                LengthCat02ReportPart = _catAdjustmentMultiplier * System.MathF.Sqrt((_tabletReport[0] - _tabletReport[1]).Length() + (_tabletReport[1] - _tabletReport[2]).Length() - (_tabletReport[0] - _tabletReport[2]).Length()) * _catAdjustmentMultiplier10[_lengthCategory, LengthCat12Report];
                                //System.Console.WriteLine("_catAdjustmentMultiplier10 " + _lengthCategory + "] " + _catAdjustmentMultiplier10[_lengthCategory, LengthCat12Report]);
                            }

                            //System.Console.WriteLine(LengthCat02ReportPart + " " + System.MathF.Sqrt((_tabletReport[0] - _tabletReport[1]).Length() + (_tabletReport[1] - _tabletReport[2]).Length() - (_tabletReport[0] - _tabletReport[2]).Length()));

                            //if (LengthCat02ReportPart > 10f)

                            LengthCat02Report = System.Convert.ToInt64(LengthCat02ReportPart + 0f);
                        }
                        else
                            LengthCat02Report = 0;

                        //LengthCat02Report *= -1;

                        if (LengthCat02Report > 10)
                            LengthCat02Report = 10;
                        else if (LengthCat02Report < 0)
                            LengthCat02Report = 0;
                        //var UintFloatAvDelta = System.Convert.ToUInt32(_lengthCategoryFloatAvDelta);
                        //if (32 > UintFloatAvDelta)
                        //    _lengthCategory = _lengthCategory + 32 * UintFloatAvDelta;
                        //else
                        //System.Console.WriteLine(_lengthCategory);
                        //System.Console.WriteLine(LengthCat12Report);
                        //System.Console.WriteLine(LengthCat02Report);
                        //System.Console.WriteLine(LengthCat12Report + " " + LengthCat02Report);

                        _lengthCategory = _lengthCategory + 17 * LengthCat12Report + 17 * 11 * LengthCat02Report;
                    }
                    else
                        _lengthCategory = 2057;
                }
                //System.Console.WriteLine(_lengthCategory);

                for(int i = 0; i < 4; i++)
                    for(int j = 0; j < 4; j++)
                        _baseVectorOld[i, j] = _baseVector[i, j];

                _baseVector[0,0] = _tabletReport[0] - _tabletReport[1]; //vel/1
                _baseVector[0,1] = 0.5f * (_tabletReport[0] - _tabletReport[2]); //vel/2
                _baseVector[0,2] = 0.3333333333f * (_tabletReport[0] - _tabletReport[3]); //vel/3
                _baseVector[0,3] = 0.25f * (_tabletReport[0] - _tabletReport[4]); //vel/4

                _baseVector[1, 0] = 0.2f * (_tabletReport[0] - _tabletReport[5]); //vel/5
                _baseVector[1, 1] = 0.166666667f * (_tabletReport[0] - _tabletReport[6]); //vel/6
                _baseVector[1, 2] = 0.142857143f * (_tabletReport[0] - _tabletReport[7]); //vel/7
                _baseVector[1, 3] = 0.125f * (_tabletReport[0] - _tabletReport[8]); //vel/8

                _baseVector[2, 0] = 2f * _tabletReport[0] + _tabletReport[2] - 3f * _tabletReport[1]; //acc/1
                _baseVector[2, 1] = _tabletReport[0] - 0.5f * _tabletReport[1] - _tabletReport[2] + 0.5f * _tabletReport[3]; //acc/2
                _baseVector[2, 2] = 0.333333333f * (2f * (_tabletReport[0] - _tabletReport[3]) - _tabletReport[1] + _tabletReport[4]); //acc/3
                _baseVector[2, 3] = 0.25f * (2f * (_tabletReport[0] - _tabletReport[4]) - _tabletReport[1] + _tabletReport[5]); //acc/4

                _baseVector[3, 0] = 3f * _tabletReport[0] - 6f * _tabletReport[1] + 4f * _tabletReport[2] - _tabletReport[3]; //jerk/1
                _baseVector[3, 1] = 0.5f * ( 3f * (_tabletReport[0] - _tabletReport[1] + _tabletReport[3]) -  _tabletReport[4]) - _tabletReport[2]; //what/2
                _baseVector[3, 2] = _tabletReport[0] - _tabletReport[1] + 0.333333333f * _tabletReport[2] - _tabletReport[3] + _tabletReport[4] - 0.333333333f * _tabletReport[5]; //dude/3
                _baseVector[3, 3] = 0.25f * ( 3f * (_tabletReport[0] - _tabletReport[1] - _tabletReport[4] + _tabletReport[5]) + _tabletReport[2] - _tabletReport[6]); //stop/4

                long LengthCategory;

                if ((_predictionComposite[_lengthCategory, 0] == 0 && _predictionComposite[_lengthCategory, 1] == 0 && _predictionComposite[_lengthCategory, 2] == 0 && _predictionComposite[_lengthCategory, 3] == 1)
                    && (_bestPrediction[3, _lengthCategory, 0] == 0 && _bestPrediction[3, _lengthCategory, 1] == 0 && _bestPrediction[3, _lengthCategory, 2] == 0 && _bestPrediction[3, _lengthCategory, 3] == 1))
                {
                    if ((_predictionComposite[_lengthCategoryOld, 0] != 0 && _predictionComposite[_lengthCategoryOld, 1] != 0 && _predictionComposite[_lengthCategoryOld, 2] != 0 && _predictionComposite[_lengthCategoryOld, 3] != 1)
                         && (_bestPrediction[3, _lengthCategoryOld, 0] != 0 && _bestPrediction[3, _lengthCategoryOld, 1] != 0 && _bestPrediction[3, _lengthCategoryOld, 2] != 0 && _bestPrediction[3, _lengthCategoryOld, 3] != 1))
                        LengthCategory = _lengthCategoryOld;
                    else
                        LengthCategory = _lastCalcLengthCategory;
                }
                else
                    LengthCategory = _lengthCategory;

                Vector2 TempVector =  (_prime[_predictionComposite[LengthCategory, 0]]
                                     * ((_baseVector[0, 0] * _prime[_bestPrediction[0, LengthCategory, 0]]
                                       + _baseVector[0, 1] * _prime[_bestPrediction[0, LengthCategory, 1]]
                                      + (_baseVector[0, 2] * _prime[_bestPrediction[0, LengthCategory, 2]]
                                       + _baseVector[0, 3] * _prime[_bestPrediction[0, LengthCategory, 3]]))
                                       * _primeDivisor[_bestPrediction[0, LengthCategory, 0], _bestPrediction[0, LengthCategory, 1], _bestPrediction[0, LengthCategory, 2], _bestPrediction[0, LengthCategory, 3]])

                                       + _prime[_predictionComposite[LengthCategory, 1]]
                                     * ((_baseVector[1, 0] * _prime[_bestPrediction[1, LengthCategory, 0]]
                                       + _baseVector[1, 1] * _prime[_bestPrediction[1, LengthCategory, 1]]
                                      + (_baseVector[1, 2] * _prime[_bestPrediction[1, LengthCategory, 2]]
                                       + _baseVector[1, 3] * _prime[_bestPrediction[1, LengthCategory, 3]]))
                                       * _primeDivisor[_bestPrediction[1, LengthCategory, 0], _bestPrediction[1, LengthCategory, 1], _bestPrediction[1, LengthCategory, 2], _bestPrediction[1, LengthCategory, 3]])

                                       + _prime[_predictionComposite[LengthCategory, 2]]
                                     * ((_baseVector[2, 0] * _prime[_bestPrediction[2, LengthCategory, 0]]
                                       + _baseVector[2, 1] * _prime[_bestPrediction[2, LengthCategory, 1]]
                                      + (_baseVector[2, 2] * _prime[_bestPrediction[2, LengthCategory, 2]]
                                       + _baseVector[2, 3] * _prime[_bestPrediction[2, LengthCategory, 3]]))
                                       * _primeDivisor[_bestPrediction[2, LengthCategory, 0], _bestPrediction[2, LengthCategory, 1], _bestPrediction[2, LengthCategory, 2], _bestPrediction[2, LengthCategory, 3]])

                                       + _prime[_predictionComposite[LengthCategory, 3]]
                                     * ((_baseVector[3, 0] * _prime[_bestPrediction[3, LengthCategory, 0]]
                                       + _baseVector[3, 1] * _prime[_bestPrediction[3, LengthCategory, 1]]
                                      + (_baseVector[3, 2] * _prime[_bestPrediction[3, LengthCategory, 2]]
                                       + _baseVector[3, 3] * _prime[_bestPrediction[3, LengthCategory, 3]]))
                                       * _primeDivisor[_bestPrediction[3, LengthCategory, 0], _bestPrediction[3, LengthCategory, 1], _bestPrediction[3, LengthCategory, 2], _bestPrediction[3, LengthCategory, 3]]))

                                       * _primeDivisor[_predictionComposite[LengthCategory, 0], _predictionComposite[LengthCategory, 1], _predictionComposite[LengthCategory, 2], _predictionComposite[LengthCategory, 3]];

                if ((_predictionComposite[_lengthCategoryOld, 0] != 0 && _predictionComposite[_lengthCategoryOld, 1] != 0 && _predictionComposite[_lengthCategoryOld, 2] != 0 && _predictionComposite[_lengthCategoryOld, 3] != 1)
                 && (_bestPrediction[3, _lengthCategoryOld, 0] != 0 && _bestPrediction[3, _lengthCategoryOld, 1] != 0 && _bestPrediction[3, _lengthCategoryOld, 2] != 0 && _bestPrediction[3, _lengthCategoryOld, 3] != 1))
                    LengthCategory = _lengthCategoryOld;
                else
                    LengthCategory = _lastCalcLengthCategory;

                /*TempVector += 0.25f * (_prime[_predictionComposite[LengthCategory, 0]]
                     * ((_baseVector[0, 0] * _prime[_bestPrediction[0, LengthCategory, 0]]
                       + _baseVector[0, 1] * _prime[_bestPrediction[0, LengthCategory, 1]]
                      + (_baseVector[0, 2] * _prime[_bestPrediction[0, LengthCategory, 2]]
                       + _baseVector[0, 3] * _prime[_bestPrediction[0, LengthCategory, 3]]))
                       * _primeDivisor[_bestPrediction[0, LengthCategory, 0], _bestPrediction[0, LengthCategory, 1], _bestPrediction[0, LengthCategory, 2], _bestPrediction[0, LengthCategory, 3]])

                       + _prime[_predictionComposite[LengthCategory, 1]]
                     * ((_baseVector[1, 0] * _prime[_bestPrediction[1, LengthCategory, 0]]
                       + _baseVector[1, 1] * _prime[_bestPrediction[1, LengthCategory, 1]]
                      + (_baseVector[1, 2] * _prime[_bestPrediction[1, LengthCategory, 2]]
                       + _baseVector[1, 3] * _prime[_bestPrediction[1, LengthCategory, 3]]))
                       * _primeDivisor[_bestPrediction[1, LengthCategory, 0], _bestPrediction[1, LengthCategory, 1], _bestPrediction[1, LengthCategory, 2], _bestPrediction[1, LengthCategory, 3]])

                       + _prime[_predictionComposite[LengthCategory, 2]]
                     * ((_baseVector[2, 0] * _prime[_bestPrediction[2, LengthCategory, 0]]
                       + _baseVector[2, 1] * _prime[_bestPrediction[2, LengthCategory, 1]]
                      + (_baseVector[2, 2] * _prime[_bestPrediction[2, LengthCategory, 2]]
                       + _baseVector[2, 3] * _prime[_bestPrediction[2, LengthCategory, 3]]))
                       * _primeDivisor[_bestPrediction[2, LengthCategory, 0], _bestPrediction[2, LengthCategory, 1], _bestPrediction[2, LengthCategory, 2], _bestPrediction[2, LengthCategory, 3]])

                       + _prime[_predictionComposite[LengthCategory, 3]]
                     * ((_baseVector[3, 0] * _prime[_bestPrediction[3, LengthCategory, 0]]
                       + _baseVector[3, 1] * _prime[_bestPrediction[3, LengthCategory, 1]]
                      + (_baseVector[3, 2] * _prime[_bestPrediction[3, LengthCategory, 2]]
                       + _baseVector[3, 3] * _prime[_bestPrediction[3, LengthCategory, 3]]))
                       * _primeDivisor[_bestPrediction[3, LengthCategory, 0], _bestPrediction[3, LengthCategory, 1], _bestPrediction[3, LengthCategory, 2], _bestPrediction[3, LengthCategory, 3]]))

                       * _primeDivisor[_predictionComposite[LengthCategory, 0], _predictionComposite[LengthCategory, 1], _predictionComposite[LengthCategory, 2], _predictionComposite[LengthCategory, 3]];*/

                _newPredictionVector = TempVector * 1f;


                _pressure = tabletReport.Pressure;

                _newInterpVector = _tabletReport[0] - _anchorPos;
                _interpDistance = (double)_newInterpVector.Length();

                var PredictionValue = _predStart / (_newReportVectorLength + 1d);
                if (PredictionValue < 1)
                {
                    _reportsBehindTarget = 1d - _predMulti * (1d - PredictionValue);
                }
                else
                {
                    if (_interpDistance < _smoothRad)
                        _reportsBehindTarget = 1d - _smoothMulti * (1d - (_smoothRad) / (_interpDistance + 1d));
                    else
                        _reportsBehindTarget = 1d;
                }

                if (_reportsBehindTarget > 1)
                {
                    _newPathVector = _newInterpVector * (float)(1d / _reportsBehindTarget);
                    _newPredictionVector = _newPathVector;
                }
                else
                {
                    _newPathVector = _newInterpVector + (_newPredictionVector) * (float)(1d - _reportsBehindTarget);
                    _newPredictionVector = _newPredictionVector * (float)((1d - _reportsBehindTarget) * _curvyPathMulti);
                }

                var OldInterpVector = _interpPos - _prevInterpPos;

                if (OldInterpVector != Vector2.Zero && _newPathVector != Vector2.Zero)
                {
                    _curvyPathMulti = _setCurvyPathMulti;

                    _oldVelocityVector = OldInterpVector * (float)(_deltaReport / _interpStepTime);

                    var CurvyDot = Vector2.Dot(Vector2.Normalize(OldInterpVector), Vector2.Normalize(_newPathVector));
                    if (CurvyDot < 0)
                    {
                        CurvyDot = 0;
                        //_oldVelocityVector = _newPathVector;
                    }
                    //else if (CurvyDot < 0.75f)
                    //_oldVelocityVector = _oldVelocityVector * (CurvyDot * 1.3333333f) + (1f - CurvyDot * 1.3333333f) * _newPathVector;

                    //CurvyDot = (1f - CurvyDot); // LengthChange;

                    //_curvyPathMulti = 1f + CurvyDot * (_setCurvyPathMulti - 1f);
                    //if (CurvyDot < 0.3333333333f)

                    var LengthChange = _newPathVector.Length() / _oldVelocityVector.Length();
                    if (LengthChange > 1f)
                        LengthChange = 1f / LengthChange;
                    //LengthChange = (LengthChange - 0.5f) * 2f;
                    if (LengthChange < 0)
                        LengthChange = 0;

                    CurvyDot = System.MathF.Pow((CurvyDot * LengthChange), _curvyCorrectionExponent);

                    _oldVelocityVectorAdjusted = CurvyDot * _oldVelocityVector + (1f - CurvyDot ) * _newPathVector;


                    //CurvyDot = (1f - CurvyDot);

                    //_curvyPathMulti = 1f + LengthChange * CurvyDot * (_setCurvyPathMulti - 1f);

                    //_oldVelocityVectorAdjusted *= CurvyDot;
                }
                else
                {
                    _curvyPathMulti = 1d;

                    _oldVelocityVector = _newPathVector;
                    _oldVelocityVectorAdjusted = _newPathVector;
                }

                _interpPeriod = _curvyPathMulti * _deltaReport + _reportStepOffset;

                if (_reportsAfterPenlift > 9 && _bufferInUse == false)
                    _interpUpdateAfterTabletReport = 1;
                else if (_bufferInUse)
                    _newReportWaiting = true;

                _reportsAfterPenlift++;
            }
            else
            {
                OnEmit();
            }
        }

        protected override void UpdateState()
        {
            var ReportWatchElapsed = _reportWatch.Elapsed.TotalMilliseconds;

            _prevInterpPos = _interpPos;

            _interpStepTime = _interpWatch.Restart().TotalMilliseconds;

            var InterpProgress = (ReportWatchElapsed + _reportStepOffset) / _interpPeriod;

            if (State is ITabletReport tabletReport && ReportWatchElapsed < 1.5d * _deltaReport + 2.5d)
            {
                if (!_directInterp)
                {
                    var Weightz = ReportWatchElapsed / (_deltaReport);
                    if (Weightz > 1d)
                    {
                        Weightz = 1d;
                    }
                    var Weight = 0.5f * (float)System.Math.Sin(Weightz - 0.5d) + 0.5f;

                    //if (InterpProgress < 1)
                        _interpPos = _anchorPos + _newPathVector * (float)(InterpProgress * InterpProgress * _curvyPathMulti) + ((1f - Weight) * _oldVelocityVector + Weight * _oldVelocityVectorAdjusted) * (float)(InterpProgress * (1d - InterpProgress) * _curvyPathMulti);
                    //else
                    //    _interpPos = _anchorPos + _newPathVector * (float)(InterpProgress * InterpProgress * _curvyPathMulti) + ((1f - Weight) * _oldVelocityVector + Weight * _oldVelocityVectorAdjusted) * (float)(InterpProgress * (1d - InterpProgress) * _curvyPathMulti);
                }
                else
                {
                    _interpPos = _anchorPos + _newPathVector * (float)(InterpProgress * _curvyPathMulti);
                }


                tabletReport.Pressure = _pressure;
                tabletReport.Position = _interpPos;
                OnEmit();
            }
            else
            {
                _reportWatch.Stop();
                _penLift = true;
            }

            //Prediction stuff below, cursor movement stuff above

            if (_interpUpdateAfterTabletReport > 4)
            {
                Vector2[,] VectorComponent = new Vector2[4, _predCalcW];

                for (int i = 0; i < 4; i++)
                {
                    for (int j = 0; j < _predCalcW; j++)
                    {
                        VectorComponent[i, j] =
                            _prime[j]
                         * (_bufferBaseVector[i, 0] * _prime[_bestPrediction[i, _bufferLengthCategory, 0]]
                          + _bufferBaseVector[i, 1] * _prime[_bestPrediction[i, _bufferLengthCategory, 1]]
                         + (_bufferBaseVector[i, 2] * _prime[_bestPrediction[i, _bufferLengthCategory, 2]]
                          + _bufferBaseVector[i, 3] * _prime[_bestPrediction[i, _bufferLengthCategory, 3]]))
                          * _primeDivisor[_bestPrediction[i, _bufferLengthCategory, 0], _bestPrediction[i, _bufferLengthCategory, 1], _bestPrediction[i, _bufferLengthCategory, 2], _bestPrediction[i, _bufferLengthCategory, 3]];
                    }
                }

                Vector2[,] PartialVector = new Vector2[3, _predCalcW];

                //int[] BestPrediction = new int[] { _predictionComposite[_bufferLengthCategoryOld, 0], _predictionComposite[_bufferLengthCategoryOld, 1], _predictionComposite[_bufferLengthCategoryOld, 2], _predictionComposite[_bufferLengthCategoryOld, 3] };
                int[] BestPrediction = new int[] { 0, 0, 0, 1 };

                for (int i0 = 0; i0 < _predCalcW; i0++)
                {
                    PartialVector[0, i0] = VectorComponent[0, i0];
                    for (int i1 = 0; i1 < _predCalcW; i1++)
                    {
                        PartialVector[1, i1] = PartialVector[0, i0] + VectorComponent[1, i1];
                        for (int i2 = 0; i2 < _predCalcW; i2++)
                        {
                            PartialVector[2, i2] = PartialVector[1, i1] + VectorComponent[2, i2];
                            for (int i3 = 0; i3 < _predCalcW; i3++)
                            {
                                _compositeVectorOption[i0, i1, i2, i3] = (PartialVector[2, i2] + VectorComponent[3, i3]) * _primeDivisor[i0, i1, i2, i3]; //

                                _compositeError[_bufferLengthCategoryOld, i0, i1, i2, i3] = 0.9999f * _compositeError[_bufferLengthCategoryOld, i0, i1, i2, i3] + (((_bufferRealMovementVector - _compositeVectorOption[i0, i1, i2, i3]).Length())); //* NewReportVectorLengthDivisor);

                                if (_compositeError[_bufferLengthCategoryOld, i0, i1, i2, i3] < _compositeError[_bufferLengthCategoryOld, BestPrediction[0], BestPrediction[1], BestPrediction[2], BestPrediction[3]])
                                    BestPrediction = new int[] { i0, i1, i2, i3};
                            }
                        }
                    }
                }

                if (BestPrediction[0] != _predictionComposite[_bufferLengthCategoryOld, 0] || BestPrediction[1] != _predictionComposite[_bufferLengthCategoryOld, 1] || BestPrediction[2] != _predictionComposite[_bufferLengthCategoryOld, 2] || BestPrediction[3] != _predictionComposite[_bufferLengthCategoryOld, 3])
                {
                    //if (BestPrediction[0] == 0 && BestPrediction[1] == 0 && BestPrediction[2] == 0 && BestPrediction[3] == 0)
                    //    System.Console.WriteLine("Warning 0 0 0 0 comp");
                    _predictionComposite[_bufferLengthCategoryOld, 0] = BestPrediction[0];
                    _predictionComposite[_bufferLengthCategoryOld, 1] = BestPrediction[1];
                    _predictionComposite[_bufferLengthCategoryOld, 2] = BestPrediction[2];
                    _predictionComposite[_bufferLengthCategoryOld, 3] = BestPrediction[3];
                                    }
                //System.Console.WriteLine(_bufferLengthCategoryOld + " comp " + BestPrediction[0] + " " + BestPrediction[1] + " " + BestPrediction[2] + " " + BestPrediction[3]);

                if (_newReportWaiting)
                    _interpUpdateAfterTabletReport = 1;
                else
                    _interpUpdateAfterTabletReport = 0;

                _lastCalcLengthCategory = _bufferLengthCategoryOld;

                _bufferInUse = false;
            }
            else if (_interpUpdateAfterTabletReport > 0)
            {
                if (_bufferInUse == false)
                {
                    _newReportWaiting = false;
                    _bufferInUse = true;

                    for (int i = 0; i < 4; i++)
                        for(int j = 0; j < 4; j++)
                            _bufferBaseVector[i, j] = _baseVectorOld[i, j];

                    _bufferLengthCategoryOld = _lengthCategoryOld;
                    _bufferLengthCategory = _lengthCategoryOld;
                    _bufferRealMovementVector = _realMovementVector;
                }

                int Derp = _interpUpdateAfterTabletReport - 1;

                Vector2[,] VectorComponent = new Vector2[4, _predCalcW];

                for (int i = 0; i < 4; i++)
                {
                    for (int j = 0; j < _predCalcW; j++)
                    {
                        VectorComponent[i, j] = _baseVector[Derp, i] * _prime[j];
                    }
                }

                Vector2[,] PartialVector = new Vector2[3, _predCalcW];

                //int[] BestPrediction = new int[] { _bestPrediction[Derp, _bufferLengthCategoryOld, 0], _bestPrediction[Derp, _bufferLengthCategoryOld, 1], _bestPrediction[Derp, _bufferLengthCategoryOld, 2], _bestPrediction[Derp, _bufferLengthCategoryOld, 3] };
                int[] BestPrediction = new int[] { 0, 0, 0, 1 };

                for (int i0 = 0; i0 < _predCalcW; i0++)
                {
                    PartialVector[0, i0] = VectorComponent[0, i0];
                    for (int i1 = 0; i1 < _predCalcW; i1++)
                    {
                        PartialVector[1, i1] = PartialVector[0, i0] + VectorComponent[1, i1];
                        for (int i2 = 0; i2 < _predCalcW; i2++)
                        {
                            PartialVector[2, i2] = PartialVector[1, i1] + VectorComponent[2, i2];
                            for (int i3 = 0; i3 < _predCalcW; i3++)
                            {
                                _newPredictionVectorOption[Derp, i0, i1, i2, i3] = (PartialVector[2, i2] + VectorComponent[3, i3]) * (_primeDivisor[i0, i1, i2, i3]);

                                _predictionError[Derp, _bufferLengthCategoryOld, i0, i1, i2, i3] = 0.9999f * _predictionError[Derp, _bufferLengthCategoryOld, i0, i1, i2, i3] + System.MathF.Pow(((_bufferRealMovementVector - _newPredictionVectorOption[Derp, i0, i1, i2, i3]).Length()), _errorPowo); //* NewReportVectorLengthDivisor);

                                if (_predictionError[Derp, _bufferLengthCategoryOld, i0, i1, i2, i3] < _predictionError[Derp, _bufferLengthCategoryOld, BestPrediction[0], BestPrediction[1], BestPrediction[2], BestPrediction[3]])
                                    BestPrediction = new int[] { i0, i1, i2, i3 };
                            }
                        }
                    }
                }

                if (BestPrediction[0] != _bestPrediction[Derp, _bufferLengthCategoryOld, 0] || BestPrediction[1] != _bestPrediction[Derp, _bufferLengthCategoryOld, 1] || BestPrediction[2] != _bestPrediction[Derp, _bufferLengthCategoryOld, 2] || BestPrediction[3] != _bestPrediction[Derp, _bufferLengthCategoryOld, 3])
                {
                    //if (BestPrediction[0] != 0 && BestPrediction[1] != 0 && BestPrediction[2] != 0 && BestPrediction[3] != 0)
                    //    System.Console.WriteLine("Warning 0 0 0 0");

                    _bestPrediction[Derp, _bufferLengthCategoryOld, 0] = BestPrediction[0];
                    _bestPrediction[Derp, _bufferLengthCategoryOld, 1] = BestPrediction[1];
                    _bestPrediction[Derp, _bufferLengthCategoryOld, 2] = BestPrediction[2];
                    _bestPrediction[Derp, _bufferLengthCategoryOld, 3] = BestPrediction[3];

                    //System.Console.WriteLine(_bufferLengthCategory + " " + BestPrediction[0] + " " + BestPrediction[1] + " " + BestPrediction[2] + " " + BestPrediction[3]);
                    //System.Console.WriteLine("new");
                }

                //System.Console.WriteLine(_bufferLengthCategory + " " + "a" + Derp + " " + BestPrediction[0] + " " + BestPrediction[1] + " " + BestPrediction[2] + " " + BestPrediction[3]);

                //System.Console.WriteLine(_bufferLengthCategory + " " + BestPrediction[0] + " " + BestPrediction[1] + " " + BestPrediction[2] + " " + BestPrediction[3]);


                _interpUpdateAfterTabletReport++;
            }
        }
    }
}