-- @server
-- @include /koptilnya/libs/constants.txt
-- @include ./wire_component.txt
require('/koptilnya/libs/constants.txt')
require('./wire_component.txt')

Differential = class('Differential', WireComponent)

function Differential:initialize(config, order)
    self.power = config.Power or 1
    self.coast = config.Coast or 1
    self.finalDrive = config.FinalDrive or 1
    self.preload = config.Preload or 10
    self.order = order or 1

    self.viscousCoeff = config.ViscousCoeff or 0.9
    self.usePowerBias = config.UsePowerBias or 10

    self.distributionCoeff = config.DistributionCoeff or 1
    self.maxCorrectingTorque = config.MaxCorrectingTorque or 200

    self.avgRPM = 0

    self.leftWheelInput = 'Axle' .. self.order .. '_LeftWheel'
    self.rightWheelInput = 'Axle' .. self.order .. '_RightWheel'

    self.leftWheel = wire.ports[self.leftWheelInput]
    self.rightWheel = wire.ports[self.rightWheelInput]

    hook.add('input', 'wire_axle_update' .. self.order, function(key, val)
        if key == self.leftWheelInput then
            self.leftWheel = val
        end

        if key == self.rightWheelInput then
            self.rightWheel = val
        end
    end)
end

function Differential:getInputs()
    local inputs = {}

    inputs[self.leftWheelInput] = 'entity'
    inputs[self.rightWheelInput] = 'entity'

    return inputs
end

function Differential:linkGearbox(gbox)
    self.gearbox = gbox
end

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

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

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

        local lwInertia = self.leftWheel:getInertia():getLength()
        local rwInertia = self.rightWheel:getInertia():getLength()

        local lwtq = lwrav * lwInertia
        local rwtq = rwrav * 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.maxCorrectingTorque,
            self.maxCorrectingTorque)
        local rwCorrectingTorque = math.clamp(rwDampingTorque * rwPowerCoeff, -self.maxCorrectingTorque,
            self.maxCorrectingTorque)

        local gboxTorque = self.gearbox and self.gearbox.torque or 0

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

        self.leftWheel:applyTorque(lwCorrectedTorque * -self.leftWheel:getRight() * TICK_INTERVAL)
        self.rightWheel:applyTorque(rwCorrectedTorque * self.rightWheel:getRight() * TICK_INTERVAL)
    end
end