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

Slave = class('Slave', WireComponent)

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

    self.lock = options.Lock or 0
    self.camber = options.Camber or 0
    self.caster = options.Caster or 0
    self.ackermann = options.Ackermann or 0
    self.toe = options.Toe or 0
    self.offset = options.Offset or 0
    self.isRight = options.IsRight or false
    self.direction = options.Direction or 1

    self.order = options.Order or 0

    self.entity = nil
    self.wheel = nil
    self.base = options.Base or nil

    self.steer = 0
    self.steeringAngle = 0
    self.lateralVel = 0
    self.lateralForce = 0
    self.selfAligningTorque = 0
    self.correctingAngle = 0
end

function Slave:getInputs()
    local inputs = {}
    inputs['SL_' .. self.order] = 'entity'
    inputs['W_' .. self.order] = 'entity'

    return inputs
end

function Slave:getOutputs()
    local outputs = {}

    outputs['SL_' .. self.order .. '_LateralForce'] = 'number'
    outputs['SL_' .. self.order .. '_SelfAligningTq'] = 'number'
    outputs['SL_' .. self.order .. '_CorrectingAngle'] = 'number'
    outputs['W_' .. self.order .. '_LateralVel'] = 'number'

    return outputs
end

function Slave:updateOutputs()
    wire.ports['SL_' .. self.order .. '_LateralForce'] = self.lateralForce
    wire.ports['SL_' .. self.order .. '_SelfAligningTq'] = self.selfAligningTorque
    wire.ports['SL_' .. self.order .. '_CorrectingAngle'] = self.correctingAngle
    wire.ports['W_' .. self.order .. '_LateralVel'] = self.lateralVel
end

function Slave:update()
    self.entity = wire.ports['SL_' .. self.order]
    self.wheel = wire.ports['W_' .. self.order]

    if isValid(self.entity) and isValid(self.base) and isValid(self.wheel) then
        local wheelMass = self.wheel:getMass()
        local wheelInertia = self.wheel:getInertia():getLength()
        local wheelRadius = self.wheel:getModelRadius() / UNITS_PER_METER
        local wheelFriction = self.wheel:getFriction()
        local lateralVel = self.wheel:worldToLocal(self.wheel:getVelocity() + self.wheel:getPos())[2]

        self.lateralVel = lateralVel

        if math.abs(self.steer) > 0 then
            self.correctingAngle = 0
        else
            self.lateralForce = wheelFriction * wheelMass * lateralVel / TICK_INTERVAL /
                                    UNITS_PER_METER
            self.selfAligningTorque = self.lateralForce * wheelRadius * 2
            self.correctingAngle = self.selfAligningTorque / wheelInertia * TICK_INTERVAL
        end

        if self.isRight then
            self.correctingAngle = self.correctingAngle * -1
        end

        self.correctingAngle = 0

        if not self.entity:isFrozen() and not self.entity:isPlayerHolding() then
            self.entity:setFrozen(1)
        end

        local inc = self.steer * self.lock * self.direction
        self.steeringAngle = math.clamp(self.steeringAngle + inc + self.correctingAngle, -self.lock, self.lock)

        local camber = self.isRight and -self.camber or self.camber
        local steerRatio = self.lock ~= 0 and self.steeringAngle / self.lock or 0
        local caster = steerRatio * self.caster
        local toe = self.isRight and self.toe or -self.toe
        local ackermann = 0

        if steerRatio > 0 then
            ackermann = self.isRight and -steerRatio * self.ackermann or steerRatio * self.ackermann
        elseif steerRatio < 0 then
            ackermann = self.isRight and steerRatio * self.ackermann or -steerRatio * self.ackermann
        end

        -- Dividing ackermann by 2 because ackermann angle is difference between wheels angle
        -- So if we don't divide it by 2, the result angle will be 2 times more than expected
        local angle = Angle(0, self.offset + self.steeringAngle + toe + ackermann / 2, camber - caster)
        local localizedAngle = self.base:localToWorldAngles(angle)

        self.entity:setAngles(localizedAngle)
    end
end