-- @include ../libs/constants.txt
-- @include ../libs/table.txt
require('../libs/constants.txt')
require('../libs/table.txt')

Differential = class('Differential')

function Differential:initialize(options)
    options = options or {}

    self.power = options.Power or 1
    self.coast = options.Coast or 1
    self.preload = options.Preload or 10
    self.viscousCoeff = options.ViscousCoeff or 0.9
    self.usePowerBias = options.UsePowerBias or 10
    self.axle = options.Axle or Vector(1, 0, 0)
    self.distributionCoeff = options.DistributionCoeff or 1

    self.avgRPM = 0

    self._leftWheel = nil
    self._rightWheel = nil

    self.lwInertia = Vector(0)
    self.rwInertia = Vector(0)

    self.lwav = 0
    self.rwav = 0

    self.lwtq = 0
    self.rwtq = 0

    self.limitTorque = 250
end

function Differential:update()
    if isValid(self._leftWheel) and isValid(self._rightWheel) then
        self.lwav = math.rad(self._leftWheel:getAngleVelocity()[2])
        self.rwav = math.rad(self._rightWheel:getAngleVelocity()[2]) * -1

        self.avgRPM = (self.lwav + self.rwav) / 2 * RAD_TO_RPM

        -- RAV = relative anglar velocity
        local lwrav = self.rwav - self.lwav
        local rwrav = self.lwav - self.rwav

        local lwtq = lwrav * self.lwInertia
        local rwtq = rwrav * self.rwInertia

        -- Calculating damping torque  here
        local lwDampingTorque = self.viscousCoeff * lwtq
        local rwDampingTorque = self.viscousCoeff * rwtq

        -- Whether use diff power or diff coast
        local usePower = self._gearbox and self._gearbox.torque > self.usePowerBias or false
        local usedCoeff = usePower and self.power or self.coast

        local lwPowerCoeff = usedCoeff
        local rwPowerCoeff = usedCoeff

        -- this could probably be shortened?
        -- if lwtq - rwtq > self.preload then
        --     lwPowerCoeff = usedCoeff
        --     rwPowerCoeff = usedCoeff
        -- end

        -- if rwtq - lwtq > self.preload then
        --     lwPowerCoeff = usedCoeff
        --     rwPowerCoeff = usedCoeff
        -- end

        local lwCorrectingTorque = math.clamp(lwDampingTorque * lwPowerCoeff, -self.limitTorque, self.limitTorque)
        local rwCorrectingTorque = math.clamp(rwDampingTorque * rwPowerCoeff, -self.limitTorque, self.limitTorque)

        local gboxTorque = self._gearbox and self._gearbox.torque or 0 -- 6289.245

        local lwCorrectedTorque = lwCorrectingTorque * 600 + gboxTorque * self.distributionCoeff * 52.49
        local rwCorrectedTorque = rwCorrectingTorque * 600 + gboxTorque * self.distributionCoeff * 52.49

        self.lwtq = lwCorrectingTorque
        self.rwtq = rwCorrectingTorque

        self._leftWheel:applyTorque(lwCorrectedTorque * -self._leftWheel:getRight() * TICK_INTERVAL)
        self._rightWheel:applyTorque(rwCorrectedTorque * self._rightWheel:getRight() * TICK_INTERVAL)
    end
end

function Differential:linkWheels(left, right)
    if isValid(left) and isValid(right) then
        self._leftWheel = left
        self._rightWheel = right

        self.lwInertia = left:getInertia():getLength()
        self.rwInertia = right:getInertia():getLength()
    end
end

function Differential:linkGearbox(gbox)
    if gbox ~= nil then
        self._gearbox = gbox

        gbox:linkDiff(self)
    end
end