package pid

import (
	"time"
)

type controller struct {
	Kp          float64
	Ki          float64
	Kd          float64
	lastTime    time.Time
	SetPoint    float64
	SampleTime  time.Duration
	WindupGuard float64
	Output      float64

	pTerm     float64
	iTerm     float64
	dTerm     float64
	lastError float64
}

/*
PID Controller
*/
type Controller interface {
	GetKp() float64
	GetKi() float64
	GetKd() float64
	GetSetPoint() float64
	GetSampleTime() time.Duration
	GetWindupGuard() float64
	GetOutput() float64

	SetKp(float64)
	SetKi(float64)
	SetKd(float64)
	SetSetPoint(float64)
	SetSampleTime(time.Duration)
	SetWindupGuard(float64)
	SetOutput(float64)

	Clear()
	Update(float64) float64
}

/*
create PID Controller
*/
func New(proportionalGain float64, integralGain float64, derivativeGain float64) Controller {
	return &controller{
		Kp:          proportionalGain,
		Ki:          integralGain,
		Kd:          derivativeGain,
		WindupGuard: 20.0,
		lastTime:    time.Now(),
	}
}

/*
Clears PID computations and coefficients
*/
func (c *controller) Clear() {
	c.SetPoint = 0.0
	c.pTerm = 0.0
	c.iTerm = 0.0
	c.dTerm = 0.0
	c.lastError = 0.0
	c.WindupGuard = 20.0
	c.Output = 0.0
}

/*
Calculates PID value for given reference feedback
.. math::
	u(t) = K_p e(t) + K_i \int_{0}^{t} e(t)dt + K_d {de}/{dt}

.. figure:: images/pid_1.png
	:align:   center

	Test PID with Kp=1.2, Ki=1, Kd=0.001 (test_pid.py)
*/
func (c *controller) Update(feedbackValue float64) float64 {
	var (
		errorValue            = c.SetPoint - feedbackValue
		currentTime           = time.Now()
		deltaTime, deltaError = currentTime.Sub(c.lastTime), errorValue - c.lastError
	)

	if deltaTime >= c.SampleTime {
		c.pTerm = c.Kp * errorValue
		c.iTerm += errorValue * deltaTime.Seconds()

		if c.iTerm < -c.WindupGuard {
			c.iTerm = -c.WindupGuard
		} else if c.iTerm > c.WindupGuard {
			c.iTerm = c.WindupGuard
		}

		c.dTerm = 0.0
		if deltaTime.Seconds() > 0.0 {
			c.dTerm = deltaError / deltaTime.Seconds()
		}
		c.lastTime = currentTime
		c.lastError = errorValue

		c.Output = c.pTerm + (c.Ki * c.iTerm) + (c.Kd * c.dTerm)
	}
	return c.Output
}

func (c *controller) GetKp() float64               { return c.Kp }
func (c *controller) GetKi() float64               { return c.Ki }
func (c *controller) GetKd() float64               { return c.Kd }
func (c *controller) GetSetPoint() float64         { return c.SetPoint }
func (c *controller) GetSampleTime() time.Duration { return c.SampleTime }
func (c *controller) GetWindupGuard() float64      { return c.WindupGuard }
func (c *controller) GetOutput() float64           { return c.Output }

func (c *controller) SetKp(v float64)               { c.Kp = v }
func (c *controller) SetKi(v float64)               { c.Ki = v }
func (c *controller) SetKd(v float64)               { c.Kd = v }
func (c *controller) SetSetPoint(v float64)         { c.SetPoint = v }
func (c *controller) SetSampleTime(v time.Duration) { c.SampleTime = v }
func (c *controller) SetWindupGuard(v float64)      { c.WindupGuard = v }
func (c *controller) SetOutput(v float64)           { c.Output = v }