local MODEL_PATH = "crazyflie/cf2_assembly.glb"

local DRONE_HEIGHT = 0.028
local DRONE_RADIUS = 0.05
local DRONE_MASS = 0.041

local PROP_CENTER = 0.031
local PROP_HEIGHT = 0.02

local PROP_MESHES = {
    { "prop.004", 1, 1 },   -- front left
    { "prop.001", 1, -1 },  -- front right
    { "prop.003", -1, 1 },  -- rear left
    { "prop.002", -1, -1 }, -- rear right
}

local function on_load(vehicle, meshes)
    -- log.info("UAV model loaded")

    vehicle:add_collider {
        shape = {
            type = "cylinder",
            half_height = DRONE_HEIGHT / 2,
            radius = DRONE_RADIUS,
        },
        position = { x = 0, y = 0, z = DRONE_HEIGHT / 2 },
        friction = 0.9,
        mass = DRONE_MASS,
    }

    -- vehicle:add_camera {
    --     id = 0,
    --     position = { x = 0, y = 0, z = 0. },
    --     rotation = { yaw = 0 },
    -- }

    for i, mesh_desc in ipairs(PROP_MESHES) do
        local name = mesh_desc[1]
        local dx = mesh_desc[2]
        local dy = mesh_desc[3]

        if meshes[name] == nil then
            sim.log.error("propeller mesh not found: " .. name)
            continue
        end

        vehicle:add_propeller(meshes[name], {
            chassis_connection_cs = {
                x = dx * PROP_CENTER,
                y = dy * PROP_CENTER,
                z = PROP_HEIGHT,
            },
            direction_cs = (i == 2 or i == 3) and "cw" or "ccw",
            axle_cs = { z = 1 },
            thrust_constant = 4e-05,
            torque_constant = 2.4e-06,
            max_velocity = 200.,
            max_torque = 30.,
        })
    end
end

return {
    model = MODEL_PATH,
    on_load = on_load,
    limits = {
        max_force = 0.8,
        max_linvel = 20,
        max_angvel = 40,
    },
}
