-- @name Grip Steering
-- @author DarkSupah
-- @server
-- @include ./slave.txt
Steering = class('Steering')

require('./slave.txt')

function inrange(val, min, max)
    return val >= min and val <= max
end

function isNaN(val)
    return val ~= val
end

function vectorIsNaN(vec)
    return isNaN(vec[1]) or isNaN(vec[2]) or isNaN(vec[3])
end

function getLocalVelocity(ent)
    return ent:worldToLocal((ent:getVelocity() + ent:getPos()))
end

local options = {
    Camber = -2,
    Caster = 5,
    Ackermann = 15,
    Lock = 45,

    ZeroThreshold = 2,

    StartSpeedThreshold = 40,
    EndSpeedThreshold = 150,
    SpeedCoeff = 0.2,

    StartCorrectionThreshold = 5,
    EndCorrectionThreshold = 40,
    CorrectionOnCoeff = 0.6,
    CorrectionOffCoeff = 1,
    CorrectionOnLerp = 0.3,
    CorrectionOffLerp = 0.6
}

function Steering:_adjustPorts()
    local inputs = {
        Axles = 'table',
        Base = 'entity',
        LeftSlave = 'entity',
        RightSlave = 'entity'
    }

    local outputs = {
        SteerAngle = 'number',
        TargetAngle = 'number',
        CorrectionAngle = 'number'
    }

    wire.adjustPorts(inputs, outputs)
end

function Steering:_createSlaves()
    local entities = {wire.ports.LeftSlave, wire.ports.RightSlave}
    local slaves = {}

    for k, v in pairs(entities) do
        local slave = Slave:new({
            Entity = v,
            IsLeft = k % 2 == 1,
            Offset = 180,
            Base = wire.ports.Base,
            Camber = self.slavesConfig.Camber,
            Caster = self.slavesConfig.Caster,
            Ackermann = self.slavesConfig.Ackermann
        })

        table.insert(self.slaves, slave)
    end
end

function Steering:_updateOutputs()
    wire.ports.TargetAngle = self.targetAngle
    wire.ports.SteerAngle = self.angle
    wire.ports.CorrectionAngle = self.correction
end

function Steering:_getCorrectionGradient(speed)
    return math.clamp(math.remap(speed, self.startCorrectionThreshold, self.endCorrectionThreshold, 0, 1), 0, 1)
end

function Steering:getBaseSpeed()
    local MPH = self.base:getVelocity():getLength() * 3600 / 63360
    local KPH = MPH * 1.609

    return KPH
end

function Steering:_getSpeedCorrectionGradient(speed)
    return math.clamp(math.remap(speed, self.startSpeedThreshold, self.endSpeedThreshold, 0, 1), 0, 1)
end

function Steering:_getCorrectionAngle()
    local localVel = getLocalVelocity(self.base)
    local forwardVel = (localVel * Vector(1, 0, 0)):getNormalized()
    local planarVec = (localVel * Vector(1, 1, 0)):getNormalized()

    forwardVel = vectorIsNaN(forwardVel) and Vector(0, 0, 0) or forwardVel
    planarVec = vectorIsNaN(planarVec) and Vector(0, 0, 0) or planarVec

    local cross = forwardVel:cross(planarVec)

    return math.deg(math.acos(forwardVel:dot(planarVec))) * math.sign(localVel[2])
end

function Steering:initialize(options)
    self:_adjustPorts()

    options = options or {}

    self.base = wire.ports.Base

    self.slavesConfig = {
        Camber = options.Camber,
        Caster = options.Caster,
        Ackermann = options.Ackermann
    }

    self.lock = options.Lock

    self.targetAngle = 0
    self.angle = 0

    self.zeroThreshold = options.ZeroThreshold

    self.speedCoeff = options.SpeedCoeff
    self.startSpeedThreshold = options.StartSpeedThreshold
    self.endSpeedThreshold = options.EndSpeedThreshold

    self.startCorrectionThreshold = options.StartCorrectionThreshold
    self.endCorrectionThreshold = options.EndCorrectionThreshold
    self.correctionOnCoeff = options.CorrectionOnCoeff
    self.correctionOffCoeff = options.CorrectionOffCoeff

    self.correctionOnLerp = options.CorrectionOnLerp
    self.correctionOffLerp = options.CorrectionOffLerp

    self.correction = 0

    self.slaves = {}

    self:_createSlaves()
end

function Steering:update()
    local horizontal = wire.ports.Axles.Horizontal.Value
    local horizontalTarget = wire.ports.Axles.Horizontal.Target

    local baseSpeedKMH = self:getBaseSpeed()
    local angleCorrection = self:_getCorrectionGradient(baseSpeedKMH)
    local speedCorrection = self.angle * self:_getSpeedCorrectionGradient(baseSpeedKMH) * self.speedCoeff

    self.targetAngle = horizontal * self.lock

    local correctionCoeff = horizontalTarget ~= 0 and self.correctionOnCoeff or self.correctionOffCoeff
    local correctionLerpCoeff = horizontalTarget ~= 0 and self.correctionOnLerp or self.correctionOffLerp
    local correction = self:_getCorrectionAngle() * angleCorrection * correctionCoeff
    self.correction = math.lerp(correctionLerpCoeff, self.correction, isNaN(correction) and 0 or correction)

    local possibleAngle = math.clamp(horizontal * self.lock + speedCorrection + self.correction, -self.lock, self.lock)

    if inrange(possibleAngle, -self.zeroThreshold, self.zeroThreshold) then
        self.angle = 0
    else
        self.angle = possibleAngle
    end

    for k, v in pairs(self.slaves) do
        v:rotate(self.angle)
    end

    self:_updateOutputs()
end

local steering = Steering:new(options)

hook.add('think', 'SteeringUpdate', function()
    steering:update()
end)