using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace DroneClient.utils
{
    internal class AdaptivePID
    {
        //Initial
        private float P = 1;
        private float I = 0.2f;
        private float D = 0.005f;

        //Adaptive
        private float aP = 1;
        private float aI = 0.2f;
        private float aD = 0.005f;

        private float P_min = 0; private float P_max = 10;
        private float I_min = 0; private float I_max = 1;
        private float D_min = 0; private float D_max = 0.1f;

        private float alpha = 0.001f;
        private float beta = 0.001f;
        private float gamma = 0.0005f;

        private float prev_error = float.NaN;
        private float integral = 0;

        public AdaptivePID()
        {
            alpha = 0; beta = 0; gamma = 0;
        }

        public AdaptivePID(float p, float i, float d, float alpha = 0.001f, float beta = 0.0005f, float gamma = 0.00005f)
        {
            P = p; aP = p;
            I = i; aI = i;
            D = d; aD = d;
            this.alpha = alpha;
            this.beta = beta;
            this.gamma = gamma;
        }

        public float process(float error, float T)
        {
            if (T <= 0) return 0;
            integral += error * T;
            float derivative = 0;
            if (float.IsFinite(prev_error))
            {
                derivative = (error - prev_error) / T;
            }
            prev_error = error;

            float dP = -alpha * error * error;
            float dI = -beta * error * integral;
            float dD = -gamma * error * derivative;

            aP += dP; aI += dI; aD += dD;

            aP = constrain(aP, P_min, P_max);
            aI = constrain(aI, I_min, I_max);
            aD = constrain(aD, D_min, D_max);
            integral = constrain(integral, -10, 10);
            float output = aP * error + aI * integral + aD * derivative;
            
            return output;
        }

        public void reset()
        {
            integral = 0;
            prev_error = 0;
            aP = P; aI = I; aD = D;
        }

        static float constrain(float val, float min, float max)
        {
            if (min >= max) return val;
            if(val < min) return min;
            if(val > max) return max;
            return val;
        }
    }
}