local aubo = require("aubo")
local sched = aubo.sched
local math = aubo.math

local threshold_ = {50.0,50.0,50.0,10.0,10.0,10.0}

local force_collision = sched.thread(function()
    while true do
        local is_break = false
        local force = getTcpForce()
        local pose = getTcpPose()
        local tcp_force = math.forceTrans(math.poseInverse(pose), force)
        for i = 1, 6 do
            if math.abs(tcp_force[i]) > threshold_[i] then
                textmsg("A collision occurred in the "..i.." direction")
                is_break = true
                break
            end
        end
        if is_break then
            popup(4, "warning", "The tool sensor detected the collision", 0)
            pause()
        end
        sched.sleep(0.005)
        sched.sync()
    end
end, "force_collision")

function enable_force_collision(threshold) 
    if hasTcpForceSensor() then
        threshold_ = threshold
        sched.run(force_collision)
        textmsg("Enable force collision successful.")
    else
        textmsg("Enable force collision failure.")
    end
end

function disable_force_collision() 
    sched.kill(force_collision)
    textmsg("Disnable force collision successful.")
end