-- @include /koptilnya/libs/wire_component.txt
-- @include /koptilnya/libs/constants.txt
-- @include /koptilnya/libs/entity.txt
require('/koptilnya/libs/wire_component.txt')
require('/koptilnya/libs/constants.txt')
require('/koptilnya/libs/entity.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 = options.Slave
    self.wheel = options.Wheel
    self.base = options.Base or nil

    self.steer = 0
    self.steeringAngle = 0
    self.lateralVel = 0
    self.baseLngVel = 0
    self.lateralForce = 0
    self.selfAligningTorque = 0
    self.correctingAngle = 0
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'
    outputs['SL_' .. self.order .. '_SteeringAngle'] = 'number'
    outputs['SL_' .. self.order .. '_BaseLngVel'] = '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
    wire.ports['SL_' .. self.order .. '_SteeringAngle'] = self.steeringAngle
    wire.ports['SL_' .. self.order .. '_BaseLngVel'] = self.baseLngVel
end

function Slave:update()
    if isValid(self.entity) and isValid(self.base) and isValid(self.wheel) then
        if not self.entity:isFrozen() and not self.entity:isPlayerHolding() then
            self.entity:setFrozen(1)
        end

        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 wheelLocalVel = getLocalVelocity(self.wheel)
        local baseLocalVel = getLocalVelocity(self.base)
        local lateralVel = wheelLocalVel[2]
        local baseLngVel = baseLocalVel[1]

        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

        self.lateralForce = wheelFriction * wheelMass * lateralVel / UNITS_PER_METER
        self.selfAligningTorque = self.lateralForce * wheelRadius

        self.correctingAngle = self.selfAligningTorque / wheelInertia * math.sin(math.rad(math.abs(self.caster * 2))) *
                                   math.sign(baseLngVel) * (self.isRight and -1 or 1)

        local inc = self.steer * self.lock * self.direction * math.cos(math.rad(math.abs(self.caster * 2)))
        self.steeringAngle = math.clamp(self.steeringAngle + inc + self.correctingAngle, -self.lock, self.lock)

        -- 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)

        self.lateralVel = lateralVel
        self.baseLngVel = baseLngVel
    end
end