--@include ./powertrain_component.txt
--@include /koptilnya/libs/constants.txt
--@include /koptilnya/libs/entity.txt

require('./powertrain_component.txt')
require('/koptilnya/libs/constants.txt')
require('/koptilnya/libs/entity.txt')

Wheel = class('Wheel', PowertrainComponent)

function Wheel:initialize(config, prefix)
    PowertrainComponent.initialize(self)

    self.prefix = prefix
    self.rollingResistance = config.RollingResistance or -50
    self.direction = config.Direction or 1

    self.wireInputs = {
        [prefix .. 'Wheel'] = 'entity'
    }
    self.wireOutputs = {
        [prefix .. 'WheelRPM'] = 'number',
        [prefix .. 'WheelTorque'] = 'number'
    }

    self.entity = NULL_ENTITY

    hook.add('input', 'vehicle_wheel_update' .. prefix, function(key, val)
        if key == prefix .. 'Wheel' then
            self.entity = val

            if not isValid(val) then
                return
            end

            if config.CalculateInertia then
                self.entity:setInertia(Vector(7.7))
                --self.entity:setInertia(calculateWheelInertia(val))
            end

            self.inertia = self.entity:getInertia().y
        end
    end)
end

function Wheel:getRadius()
    if not isValid(self.entity) then
        return 0
    end

    return self.entity:getModelRadius() * UNITS_TO_METERS
end

function Wheel:updateWireOutputs()
    wire.ports[self.prefix .. 'WheelRPM'] = self:getRPM()
    wire.ports[self.prefix .. 'WheelTorque'] = self.torque
end

function Wheel:queryAngularVelocity()
    return self.angularVelocity
end

function Wheel:forwardStep(torque, inertia)
    if not isValid(self.entity) then
        return 0
    end

    local initAngularVelocity = self.angularVelocity

    self.entity:setInertia(Vector(inertia))
    self.torque = math.deg(torque) * 1.33 * -self.direction
    self.entity:applyTorque(self.torque * TICK_INTERVAL * self.entity:getRight())
    self.angularVelocity = math.rad(self.entity:getAngleVelocity().y) * self.direction

    local tq = (self.angularVelocity - initAngularVelocity) * inertia * TICK_INTERVAL

    return self.rollingResistance - (tq - torque)
end

--local physObj = self.entity:getPhysicsObject()
--local load = physObj:getStress()
--local frictionSnapshot = physObj:getFrictionSnapshot()
--local forwardFrictionSpeed = 0
--local contactVelocity = physObj:getVelocityAtPoint(self.entity:getPos() + Vector(0, 0, self.entity:getModelRadius())) / 39.37
--
--if #table.getKeys(frictionSnapshot) > 0 then
--    local data = frictionSnapshot[1]
--    local forwardDir = data['Normal']:cross(self.entity:getRight() * self.direction):getNormalized()
--
--    forwardFrictionSpeed = contactVelocity:dot(forwardDir)
--end
--
--local errorTorque = (self.angularVelocity - forwardFrictionSpeed / self:getRadius()) * inertia / TICK_INTERVAL
--local rollingResistance = -40
--local deltaOmegaTorque = (self.angularVelocity - initialAngularVelocity) * self.inertia / TICK_INTERVAL
--
--return rollingResistance - deltaOmegaTorque