M_PI = 3.1415926

--[[
------------------------------
计算两点间距离
------------------------------
--]]
Point_judgment = false

--计算两个位姿的位置距离
function distance_cal(p1, p2)
	local distance = poseDistance(p1, p2);
	return distance
end

--获取当前位姿，并与传进来的位姿做求distance，返回distance
function get_distance_from_start(pre_p)
	sleep(0.1)
	local now_pose = getTcpPose()
	local result = distance_cal(now_pose, pre_p)
	return result
end
--END计算两点间距离


--[[
------------------------------
运动相关
------------------------------
--]]
--输入用户坐标系相对于tcp的偏移、用户坐标系下的坐标；输出base下的坐标
function targetPoseinUsercoordToBase(usercoord_in_tcp,target_pose_in_usercoord)
    local tcp_pose = getTcpPose()
    local usercoord_in_base = poseTrans(tcp_pose, usercoord_in_tcp)
    local target_pose_in_base = poseTrans(usercoord_in_base, target_pose_in_usercoord)
    return target_pose_in_base
end

--在用户坐标系(相对与tcp)的Y轴上进行左右来回摇摆移动，distance为摆幅，search_nums为一共摇摆几次
function searchY_move(usercoord_in_tcp,distance,search_nums,a,v)
    for i=1 ,search_nums  do
        local target_pose_in_usercoord1 = {0.0, -distance, 0.0, 0.0, 0.0, 0.0}
        local target_pose_in_base1 = targetPoseinUsercoordToBase(usercoord_in_tcp,
                                                      target_pose_in_usercoord1)
        moveLine(target_pose_in_base1, a, v, 0, 0)
        
        local target_pose_in_usercoord2 = {0.0, distance, 0.0, 0.0, 0.0, 0.0}
        local  target_pose_in_base2 = targetPoseinUsercoordToBase(usercoord_in_tcp,
                                                      target_pose_in_usercoord2)
         moveLine(target_pose_in_base2, a, v, 0, 0)
    end
end

--在用户坐标系(相对与tcp)的X轴上进行左右来回摇摆移动，distance为摆幅，search_nums为一共摇摆几次
function searchX_move(usercoord_in_tcp,distance,search_nums,a,v)
    for i=1 ,search_nums  do
        local target_pose_in_usercoord1 = {-distance, 0.0, 0.0, 0.0, 0.0, 0.0}
        local target_pose_in_base1 = targetPoseinUsercoordToBase(usercoord_in_tcp,
                                                      target_pose_in_usercoord1)
        moveLine(target_pose_in_base1, a, v, 0, 0)
		sleep(0)
        
        local target_pose_in_usercoord2 = {distance, 0.0, 0.0, 0.0, 0.0, 0.0}
        local  target_pose_in_base2 = targetPoseinUsercoordToBase(usercoord_in_tcp,
                                                      target_pose_in_usercoord2)
        moveLine(target_pose_in_base2, a, v, 0, 0)
		sleep(0)
    end
end

--在用户坐标系(相对与tcp)的X轴上移动distance
function usercoord_X_move(usercoord_in_tcp,distance,a,v)
	local target_pose_in_usercoord = {distance, 0.0, 0.0, 0.0, 0.0, 0.0}
    local target_pose_in_base = targetPoseinUsercoordToBase(usercoord_in_tcp,
                                                      target_pose_in_usercoord)
    moveLine(target_pose_in_base, a, v, 0, 0)
end

--在用户坐标系(相对与tcp)的Y轴上移动distance
function usercoord_Y_move(usercoord_in_tcp,distance,a,v)
	local target_pose_in_usercoord = {0.0, distance, 0.0, 0.0, 0.0, 0.0}
    local target_pose_in_base = targetPoseinUsercoordToBase(usercoord_in_tcp,
                                                      target_pose_in_usercoord)
    moveLine(target_pose_in_base, a, v, 0, 0)
end

--在用户坐标系(相对与tcp)的Z轴上移动distance
function usercoord_Z_move(usercoord_in_tcp,distance,a,v)
	local target_pose_in_usercoord = {0.0, 0.0, distance, 0.0, 0.0, 0.0}
    local target_pose_in_base = targetPoseinUsercoordToBase(usercoord_in_tcp,
                                                      target_pose_in_usercoord)
    moveLine(target_pose_in_base, a, v, 0, 0)
end

--在用户坐标系(相对与tcp)的Z轴上移动distance
function usercoord_XYZ_move(usercoord_in_tcp,dis_x,dis_y,dis_z,a,v)
	local target_pose_in_usercoord = {dis_x, dis_x, dis_z, 0.0, 0.0, 0.0}
    local target_pose_in_base = targetPoseinUsercoordToBase(usercoord_in_tcp,
                                                      target_pose_in_usercoord)
    moveLine(target_pose_in_base, a, v, 0, 0)
end

function move_Spiral()
-- 螺旋线第一圈半径
        SpiralParameters = {
            spiral = 0.001, -- 螺旋线步长
            helix = 0.0, -- 平面螺旋
            angle = 1* 2 * M_PI, -- 旋转2圈
            plane = 0,
            frame = getTcpPose()
        }
        --SpiralParameters.frame[3] = SpiralParameters.frame[3] - 0.001
		local a = 0.005
		local v = 0.005
        radius = 0.001 -- 螺旋线第一圈半径  
        target_pose = getTcpPose()
        --计算螺旋轨迹起点
        -- target_pose[1] = math.sqrt(2) * radius / 2 + target_pose[1]
        --target_pose[2] = math.sqrt(2) * radius / 2 + target_pose[2]
        --target_pose[3] = target_pose[3] + 0.004
        target_pose[1] = target_pose[1] + 0.001
        target_pose[2] = target_pose[2]
        target_pose[3] = target_pose[3] + 0.005
        v = 0.005
        a = 0.1
        moveLine(target_pose, a, v, 0, 0)
        textmsg(a1)
        sleep(0)
		--textmsg(a2)
        -- 螺旋运动
        moveSpiral(SpiralParameters, 0, 0.005, 0.1, 0)
end

--[[
--示例
-- 用户坐标系相对于tcp的偏移
local usercoord_in_tcp = { 0, 0, 0.0, 0, 0, 0 }
local distance = 0.001
local search_nums = 5
local a = 0.005
local v = 0.005
searchY_move(usercoord_in_tcp, distance, search_nums, a, v)
--]]
--END运动相关