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

Differential = class('Differential', WireComponent)

function Differential:initialize(config, order)
    self.finalDrive = config.FinalDrive or 1
    self.distributionCoeff = config.DistributionCoeff or 1
    self.axle = config.Axle or Vector(1, 0, 0)

    self.order = order

    self.avgRPM = 0

    self.prefix = 'Axle' .. self.order .. '_'

    self.leftWheel = wire.ports[self.prefix .. 'LeftWheel']
    self.rightWheel = wire.ports[self.prefix .. 'RightWheel']

    self.lwav = 0
    self.rwav = 0

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

        if key == self.prefix .. 'RightWheel' then
            self.rightWheel = val
        end
    end)
end

function Differential:getInputs()
    local inputs = {}

    inputs[self.prefix .. 'LeftWheel'] = 'entity'
    inputs[self.prefix .. 'RightWheel'] = 'entity'

    return inputs
end

function Differential:getOutputs()
    local outputs = {}

    outputs[self.prefix .. 'LWAV'] = 'number'
    outputs[self.prefix .. 'RWAV'] = 'number'
    outputs[self.prefix .. 'AvgRPM'] = 'number'
    outputs[self.prefix .. 'Test'] = 'number'

    return outputs
end

function Differential:updateOutputs()
    wire.ports[self.prefix .. 'LWAV'] = self.lwav
    wire.ports[self.prefix .. 'RWAV'] = self.rwav
    wire.ports[self.prefix .. 'AvgRPM'] = self.avgRPM
end

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

function Differential:getAngleVelocity()
    local lwav = math.rad(self.leftWheel:getAngleVelocity()[2])
    local rwav = math.rad(-self.rightWheel:getAngleVelocity()[2])
    local awav = (lwav + rwav) / 2

    return lwav, rwav, awav
end

function Differential:getRPM()
    local lwav, rwav, awav = self:getAngleVelocity()

    return lwav * RAD_TO_RPM, rwav * RAD_TO_RPM, awav * RAD_TO_RPM
end

function Differential:update()
    if isValid(self.leftWheel) and isValid(self.rightWheel) then
        local lwav, rwav = self:getAngleVelocity()

        local inertia = self.leftWheel:getInertia().y + self.rightWheel:getInertia().y
        local gboxTorque = self.gearbox and self.gearbox.torque or 0
        local simmetric = gboxTorque * self.distributionCoeff * self.finalDrive / 2
        local lock100 = (lwav - rwav) / 2 * inertia * TICK_INTERVAL
        
        wire.ports[self.prefix .. 'Test'] = (gboxTorque * self.distributionCoeff * self.finalDrive) - ((simmetric - lock100) + (simmetric + lock100))

        self.leftWheel:applyTorque((simmetric - lock100) * 1.33 * -self.leftWheel:getRight())
        self.rightWheel:applyTorque((simmetric + lock100) * 1.33 * self.rightWheel:getRight())

        self.lwav, self.rwav = self:getAngleVelocity()
        _, _, self.avgRPM = self:getRPM()
        self.avgRPM = self.avgRPM * self.finalDrive
    end
end