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

namespace DyeingComputer.UserClass
{
    public class PID
    {
        private double Ts;                  // Sample period in seconds
        private double K;                   // Rollup parameter
        private double b0, b1, b2;          // Rollup parameters
        private double a0, a1, a2;          // Rollup parameters
        private double y0 = 0;              // Current output
        private double y1 = 0;              // Output one iteration old
        private double y2 = 0;              // Output two iterations old
        private double e0 = 0;              // Current error
        private double e1 = 0;              // Error one iteration old
        private double e2 = 0;              // Error two iterations old

        #region 构造函数
        /// <summary>
        /// PID构造器
        /// </summary>
        /// <param name="Kp">比例增益</param>
        /// <param name="Ki">积分增益</param>
        /// <param name="Kd">微分增益</param>
        /// <param name="N">微分滤波系数</param>
        /// <param name="OutputUpperLimit">控制器输出上限</param>
        /// <param name="OutputLowerLimit">控制器输出下限</param>
        public PID(double Kp, double Ki, double Kd, double N, double OutputUpperLimit, double OutputLowerLimit)
        {
            this.Kp = Kp;
            this.Ki = Ki;
            this.Kd = Kd;
            this.N = N;
            this.OutputUpperLimit = OutputUpperLimit;
            this.OutputLowerLimit = OutputLowerLimit;
        }

        public PID()
        {
            this.Kp = 0.5;
            this.Ki = 0.1;
            this.Kd = 0.2;
            this.N = 1;
            this.OutputUpperLimit = 65530;
            this.OutputLowerLimit = 0;
        }
        #endregion
        /// <summary>
        /// PID迭代器，在每个采样周期调用此函数以获取当前控制器输出。
        /// 设定点和过程值应使用相同的单位。
        /// </summary>
        /// <param name="setPoint">设定目标值</param>
        /// <param name="processValue">当前实际值</param>
        /// <param name="ts">自上次迭代以来的时间跨度，对第一次调用使用默认采样周期</param>
        /// <returns>Current Controller Output</returns>
        public double PID_iterate(double setPoint, double processValue, TimeSpan ts)
        {
            // Ensure the timespan is not too small or zero.
            Ts = (ts.TotalSeconds >= TsMin) ? ts.TotalSeconds : TsMin;

            // Calculate rollup parameters
            K = 2 / Ts;
            b0 = Math.Pow(K, 2) * Kp + K * Ki + Ki * N + K * Kp * N + Math.Pow(K, 2) * Kd * N;
            b1 = 2 * Ki * N - 2 * Math.Pow(K, 2) * Kp - 2 * Math.Pow(K, 2) * Kd * N;
            b2 = Math.Pow(K, 2) * Kp - K * Ki + Ki * N - K * Kp * N + Math.Pow(K, 2) * Kd * N;
            a0 = Math.Pow(K, 2) + N * K;
            a1 = -2 * Math.Pow(K, 2);
            a2 = Math.Pow(K, 2) - K * N;

            // Age errors and output history
            e2 = e1;                        // Age errors one iteration
            e1 = e0;                        // Age errors one iteration
            e0 = setPoint - processValue;   // Compute new error
            y2 = y1;                        // Age outputs one iteration
            y1 = y0;                        // Age outputs one iteration
            y0 = -a1 / a0 * y1 - a2 / a0 * y2 + b0 / a0 * e0 + b1 / a0 * e1 + b2 / a0 * e2; // Calculate current output

            // Clamp output if needed
            if (y0 > OutputUpperLimit)
            {
                y0 = OutputUpperLimit;
            }
            else if (y0 < OutputLowerLimit)
            {
                y0 = OutputLowerLimit;
            }

            return y0;
        }

        /// <summary>
        /// 重置控制器历史记录，有效重置控制器。
        /// </summary>
        public void ResetController()
        {
            e2 = 0;
            e1 = 0;
            e0 = 0;
            y2 = 0;
            y1 = 0;
            y0 = 0;
        }

        /// <summary>
        /// 比例增益，如果此参数发生剧烈变化，请考虑重置控制器。
        /// </summary>
        public double Kp { get; set; }

        /// <summary>
        /// 积分增益，如果此参数发生剧烈变化，请考虑重置控制器。
        /// </summary>
        public double Ki { get; set; }

        /// <summary>
        /// 微分增益，如果此参数发生剧烈变化，请考虑重置控制器。
        /// </summary>
        public double Kd { get; set; }

        /// <summary>
        ///微分滤波系数。
        /// N越小，过滤效果越好
        /// N越大，过滤效果越差
        /// 如果此参数发生剧烈变化，请考虑重置控制器。
        /// </summary>
        public double N { get; set; }

        /// <summary>
        /// 避免除以零的最小允许采样周期！
        /// Ts值可能会在第一次迭代时被错误地设置为过低的值或零。
        /// TsMin默认设置为1毫秒。
        /// </summary>
        public double TsMin { get; set; } = 0.001;

        /// <summary>
        /// 控制器的输出上限。
        /// 是一个比输出下限大的数值。
        /// </summary>
        public double OutputUpperLimit { get; set; }

        /// <summary>
        /// 控制器的输出下限。
        /// 是一个比输出上限小的数值。
        /// </summary>
        public double OutputLowerLimit { get; set; }
    }
}