using System;

namespace ln.ethercat.controller
{
    public delegate double LoopGetProcessValueDelegate();

    public abstract class ControlLoop
    {
        public Controller Controller { get; }

        public LoopGetProcessValueDelegate GetProcessValueDelegate { get; set; }

        public double SetPoint { get; set; }
        public double ErrorValue { get; private set; }
        public double Output { get; private set; }

        public ControlLoop(Controller controller, LoopGetProcessValueDelegate getProcessValueDelegate)
        {
            Controller = controller;
            GetProcessValueDelegate = getProcessValueDelegate;
        }

        public virtual void Clear()
        {
            ErrorValue = 0;
            Output = 0;
        }
        public virtual void Loop() => Loop(GetProcessValueDelegate());
        public virtual void Loop(double processValue)
        {
            ErrorValue = SetPoint - processValue;
            Output = LoopImplementation();
        }

        protected abstract double LoopImplementation();
        protected abstract void ClearImplementation();
    }

    public class PIDControlLoop : ControlLoop
    {
        public double Kp { get; set; } = 1.0;
        public double Ki { get; set; } = 1.0;
        public double Tn { 
            get => 1.0 / Ki;
            set => Ki = (value == 0) ? 0.0 : (1.0 / value);
        }

        public double Kd { get; set; } = 1.0;
        

        public PIDControlLoop(Controller controller, LoopGetProcessValueDelegate getProcessValueDelegate)
        :base(controller, getProcessValueDelegate)
        {}

        public double Integral { get; set; }
        public double MinIntregal { get; set; } = double.MinValue;
        public double MaxIntegral { get; set; } = double.MaxValue;
        public double LastError { get; private set; }

        protected override void ClearImplementation()
        {
            Integral = Math.Clamp(0, MinIntregal, MaxIntegral);
            LastError = 0;
        }

        protected override double LoopImplementation()
        {
            double output = (Kp * ErrorValue);

            if (Ki > 0.0)
            {
                Integral += (ErrorValue * Controller.ControllerLoopInterval * Ki) * Kp;
                Integral = Math.Clamp(Integral, MinIntregal, MaxIntegral);
                output += Integral;
            }
            if (Kd != 0)
            {
                output += (Kd * (ErrorValue - LastError) * Controller.ControllerLoopInterval) * Kp;
                LastError = ErrorValue;
            }

            return output;
        }



    }


}