-- PID controller class
#include "helpers/mathfr.lua"

c_PID = {}

function c_PID:new()
    controller = {
        Kp  = 0.0,
        Ki  = 0.0,
        Kd  = 0.0,

        is_first_frame = true,
        last_error = 0.0,
        speed_coef = 0.25,
        integral_index = 1,
        integral_data = { 0.0, 0.0, 0.0, 0.0, 0.0 },
        integral_sum = 0.0,
        float_saturation = 1.0,
        output_value,

        SetParams = function(self, pGain, iGain, dGain)
            self.Kp = pGain
            self.Ki = iGain
            self.Kd = dGain
        end,

        -- From Mode
        UpdateRotation = function(self, dt, pnt, vehicleTransform, skip_y)
            local err = self:CheckError(pnt, vehicleTransform, skip_y)
            local integralG = self:CalculateIntegralError(err * dt)
            local der_measurement = 0.0

            if not self.is_first_frame then
                der_measurement = (err - self.last_error) / dt
                self.last_error = err
            else
                self.is_first_frame = false
            end

            self.output_value = err * self.Kp + integralG * self.Ki + der_measurement * self.Kd

            return math:Clamp(self.output_value, -self.float_saturation, self.float_saturation)
        end,

        CheckError = function(self, pnt, vehicleTransform, skip_y)
            if not skip_y then
                pnt[2] = vehicleTransform.pos[2]
            end
            
            local linePnt = vehicleTransform.pos
            local fwd = TransformToParentPoint(vehicleTransform, Vec(0,0,-100))
            
            local cross = VecCross(VecSub(fwd,linePnt), VecSub(pnt,linePnt))
            local d = VecLength(cross) / 1000
    
            pnt = VecSub(pnt,linePnt)
            fwd = VecSub(fwd,linePnt)
            linePnt = VecSub(linePnt,linePnt)
            local sign = (fwd[1]-linePnt[1])*(pnt[3]-linePnt[3])-(fwd[3]-linePnt[3])*(pnt[1]-linePnt[1])
            if(sign<0) then
                sign = -1
            elseif(sign>0) then
                sign = 1
            else
                sign = 0
            end
    
            return d*sign
        end,

        CalculateIntegralError = function(self, err)
            local index = self.integral_index
            self.integral_sum = self.integral_sum - self.integral_data[index]
            self.integral_sum = self.integral_sum + err
            self.integral_data[index] = err
            self.integral_index = (self.integral_index % #self.integral_data) + 1
            return self.integral_sum
        end,

        PIDUpdateSpeed = function(self, dt, desired_speed, vehicle_speed)
            local err = (desired_speed - vehicle_speed) / 3.6
            local integralG = self:CalculateIntegralError(err * dt)
            local der_measurement = 0.0

            if not self.is_first_frame then
                der_measurement = (err - self.last_error) / dt
                self.last_error = err
            else
                self.is_first_frame = false
            end

            self.output_value = err * self.Kp + integralG * self.Ki + der_measurement * self.Kd

            if self.output_value >= 0.0 then
                return math.min(self.output_value, self.float_saturation)
            else
                return -self.float_saturation
            end
        end,
    }

	setmetatable(controller, {__index = self})
    return controller
end