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

local PowertrainComponent = require('./powertrain_component.txt')
local CustomWheel = require('../wheel/wheel.txt')

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

local Wheel = class('Wheel', PowertrainComponent)

local DEBUG = true

function Wheel:initialize(vehicle, name, config)
    PowertrainComponent.initialize(self, vehicle, name, config)

    self.steerLock = config.SteerLock or 0
    self.brakePower = config.BrakePower or 0
    self.handbrakePower = config.HandbrakePower or 0

    self.rotationAxle = config.RotationAxle or Angle(0, 0, 1)

    self.direction = math.sign(math.cos(math.rad(config.Offset or 0)))

    self.wireInputs = {
        [self.name] = 'entity'
    }
    self.wireOutputs = {
        [string.format('%s_RPM', self.name)] = 'number',
        [string.format('%s_Mz', self.name)] = 'number',
        [string.format('%s_Fz', self.name)] = 'number',
    }

    self.rot = 0

    self.entity = config.Entity or NULL_ENTITY

    self.holo = self:createHolo(self.entity)

    self.customWheel = CustomWheel:new(config.CustomWheel)

    hook.add('input', 'vehicle_wheel_update' .. self.name, function(name, value)
        if name == self.name then
            self.entity = value
            self.customWheel:setEntity(value)

            if isValid(self.holo) then
                self.holo:remove()
            end

            self.holo = self:createHolo(self.entity)

            if DEBUG then
                self.debugHoloF:setPos(self.entity:localToWorld(Vector(24,0,0)))
                self.debugHoloR:setPos(self.entity:localToWorld(Vector(0,24,0)))
                self.debugHoloU:setPos(self.entity:localToWorld(Vector(0,0,0)))

                self.debugHoloF:setParent(self.entity)
                self.debugHoloR:setParent(self.entity)
                self.debugHoloU:setParent(self.entity)
            end

            if not config.Radius then
                self.customWheel.radius = self:getEntityRadius()
            end
        end
    end)

    if DEBUG then
        self.debugHoloF = holograms.create(Vector(), Angle(), 'models/sprops/rectangles_thin/size_0/rect_1_5x48x1_5.mdl')
        self.debugHoloR = holograms.create(Vector(), Angle(), 'models/sprops/rectangles_thin/size_0/rect_1_5x48x1_5.mdl')
        self.debugHoloU = holograms.create(Vector(), Angle(), 'models/sprops/rectangles_thin/size_0/rect_1_5x48x1_5.mdl')

        self.debugHoloF:setMaterial('models/debug/debugwhite')
        self.debugHoloR:setMaterial('models/debug/debugwhite')
        self.debugHoloU:setMaterial('models/debug/debugwhite')

        self.debugHoloF:setColor(Color(255, 0, 0))
        self.debugHoloR:setColor(Color(0, 255, 0))
        self.debugHoloU:setColor(Color(0, 0, 255))
    end

    self.steerVelocity = 0
end

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

    return self.entity:getModelRadius() * UNITS_TO_METERS
end

function Wheel:createHolo(entity)
    if not isValid(entity) then
        return NULL_ENTITY
    end

    local holo = holograms.create(
            entity:getPos(),
            entity:getAngles(),
            self.CONFIG.Model or ''
    )
    
    holo:setParent(entity)

    entity:setColor(Color(0,0,0,0))

    return holo
end

function Wheel:updateWireOutputs()
    wire.ports[string.format('%s_RPM', self.name)] = self.customWheel:getRPM()
    wire.ports[string.format('%s_Mz', self.name)] = self.customWheel.mz
    wire.ports[string.format('%s_Fz', self.name)] = self.customWheel.load
end

function Wheel:queryInertia()
    return self.customWheel.inertia
end

function Wheel:queryAngularVelocity()
    return self.angularVelocity
end

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

    self.customWheel.motorTorque = torque * self.direction
    self.customWheel.brakeTorque = math.max(self.brakePower * self.vehicle.brake, self.handbrakePower * self.vehicle.handbrake)

    self.customWheel:update()

    self.angularVelocity = self.customWheel.angularVelocity * self.direction

    if self.customWheel.hasHit and isValid(self.vehicle.basePhysObject) then
        local surfaceForceVector = self.customWheel.right * self.customWheel.sideFriction.force + self.customWheel.forward * self.customWheel.forwardFriction.force

        self.vehicle.basePhysObject:applyForceOffset(surfaceForceVector, self.entity:localToWorld(Vector(0, 0, -self.entity:getModelRadius())))
    end

    if isValid(self.holo) then
        if DEBUG then
            self.debugHoloF:setAngles(self.customWheel.forward:getAngle())
            self.debugHoloR:setAngles(self.customWheel.right:getAngle())
            self.debugHoloU:setAngles(self.customWheel.up:getAngle())
        end

        local entityAngles = self.entity:getAngles()

        self.rot = self.rot + self.angularVelocity * TICK_INTERVAL * self.direction

        local steerRotated = entityAngles:rotateAroundAxis(self.customWheel.up, nil, math.rad(-self.customWheel.steerAngle) + self.customWheel.toeAngle)
        local camberRotated = steerRotated:rotateAroundAxis(self.customWheel.forward, nil, -self.customWheel.camberAngle)
        local angularVelocityRotated = camberRotated:rotateAroundAxis(self.customWheel.right, nil, self.rot)

        self.holo:setAngles(angularVelocityRotated)
    end

    return self.customWheel.counterTorque
end

return Wheel