package controllers

import (
	"math"

	"git.joco.dk/snr/fermentord/internal/metrics"
)

type PIDController struct {
	kp          float64
	ki          float64
	kd          float64
	lastError   float64
	lastOutput  float64
	integration float64
	lbound      float64
	ubound      float64
}

func NewPIDController(p, i, d float64) *PIDController {
	return &PIDController{
		kp:     p, // 2.0
		ki:     i, // 0.0001
		kd:     d, // 2.0
		lbound: -15.0,
		ubound: 15.0,
	}
}

func (p *PIDController) Compute(input float64, setpoint float64) float64 {
	error := setpoint - input
	if p.lastError == 0 {
		p.lastError = error
	}

	pv := p.kp * error
	dv := -(p.kd * (error - p.lastError))
	p.lastError = error

	if p.lastOutput > p.lbound && p.lastOutput < p.ubound {
		p.integration += error
	}

	iv := p.integration * p.ki

	p.lastOutput = math.Max(math.Min(pv+iv+dv, p.ubound), p.lbound)

	metrics.PID.WithLabelValues("pv", "main").Set(pv)
	metrics.PID.WithLabelValues("dv", "main").Set(dv)
	metrics.PID.WithLabelValues("iv", "main").Set(iv)
	metrics.PID.WithLabelValues("offset", "main").Set(p.lastOutput)
	metrics.PID.WithLabelValues("setpoint", "main").Set(setpoint)

	return p.lastOutput
}