local ACCELERATION = 100
local DECELERATION = 200
local MAX_SPEED = 400
local WHEEL_BASE = 80

local UP = vmath.vector3(0, 1, 0)

function init(self)
    msg.post(".", "acquire_input_focus")

    self.left_speed = 0
    self.right_speed = 0
    self.left_fwd = false
    self.left_rev = false
    self.right_fwd = false
    self.right_rev = false
end

local function update_speed(speed, drive, dt)
    if drive ~= 0 then
        local target = drive * MAX_SPEED
        local step = ACCELERATION * dt
        if speed < target then
            return math.min(speed + step, target)
        else
            return math.max(speed - step, target)
        end
    else
        local step = DECELERATION * dt
        if speed > 0 then
            return math.max(speed - step, 0)
        else
            return math.min(speed + step, 0)
        end
    end
end

function update(self, dt)
    local left_drive  = (self.left_fwd  and 1 or 0) - (self.left_rev  and 1 or 0)
    local right_drive = (self.right_fwd and 1 or 0) - (self.right_rev and 1 or 0)

    self.left_speed  = update_speed(self.left_speed,  left_drive,  dt)
    self.right_speed = update_speed(self.right_speed, right_drive, dt)

    -- differential drive: speed difference creates rotation, average creates forward motion
    local linear_speed = (self.left_speed + self.right_speed) / 2
    local angular_vel = (self.right_speed - self.left_speed) / WHEEL_BASE

    local rot = go.get_rotation()
    rot = rot * vmath.quat_rotation_z(angular_vel * dt)
    go.set_rotation(rot)

    local p = go.get_position()
    p = p + vmath.rotate(rot, UP * linear_speed * dt)
    go.set_position(p)

end

function on_input(self, action_id, action)
    local held = action.pressed or not action.released
    if     action_id == hash("a_key") then self.left_fwd  = held
    elseif action_id == hash("q_key") then self.left_rev  = held
    elseif action_id == hash("d_key") then self.right_fwd = held
    elseif action_id == hash("e_key") then self.right_rev = held
    end
end