local M = {}

local math = math
local welding_common = require("welding_common")

local function formatPose(p)
	if type(p) ~= "table" or #p < 6 then
		return "nil/invalid"
	end
	return string.format("[%.6f, %.6f, %.6f, %.6f, %.6f, %.6f]", p[1], p[2], p[3], p[4], p[5], p[6])
end

-- =========================================================
-- Welding math helper
-- 约定：
-- 1) 位置/姿态统一使用 6D pose: {x, y, z, rx, ry, rz}
-- 2) 回退接口输入 points:
--    - 直线: { stop_point, start_point, end_point } 共 3 个 pose
--    - 圆弧: { stop_point, start_point, middle_point, end_point } 共 4 个 pose
-- 3) 回退接口返回:
--    - ok=false: 计算失败，points={}
--    - ok=true:  points 为回退路径点集合（每个元素是 6D pose）
-- =========================================================

-- =========================================================
-- 基础向量运算（仅使用 xyz 分量）
-- =========================================================
local function vec_sub(a, b)
	return {
		(a[1] or 0) - (b[1] or 0),
		(a[2] or 0) - (b[2] or 0),
		(a[3] or 0) - (b[3] or 0),
	}
end

local function vec_dot(a, b)
	return a[1] * b[1] + a[2] * b[2] + a[3] * b[3]
end

local function vec_cross(a, b)
	return {
		a[2] * b[3] - a[3] * b[2],
		a[3] * b[1] - a[1] * b[3],
		a[1] * b[2] - a[2] * b[1],
	}
end

local function vec_len(a)
	return math.sqrt(vec_dot(a, a))
end

local function vec_norm(a)
	local l = vec_len(a)
	if l < 1e-9 then
		return { 0, 0, 0 }
	end
	return { a[1] / l, a[2] / l, a[3] / l }
end

local function vec_add(a, b)
	return { a[1] + b[1], a[2] + b[2], a[3] + b[3] }
end

local function clamp(x, a, b)
	if x < a then
		return a
	end
	if x > b then
		return b
	end
	return x
end

-- =========================================================
-- 直线距离（仅 xyz）
-- =========================================================
function M.distance(p1, p2)
	return vec_len(vec_sub(p1, p2))
end

-- =========================================================
-- 三点圆弧长度（完整 MoveC）
-- 输入:
--   p1: 起点 xyz
--   p2: 中间点 xyz
--   p3: 终点 xyz
-- 返回:
--   圆弧长度（若三点近共线，则退化为 p1->p3 直线距离）
-- =========================================================
function M.arcLength(p1, p2, p3)
	local A = vec_sub(p1, p2)
	local B = vec_sub(p3, p2)

	local n = vec_cross(A, B)
	local n_len = vec_len(n)

	-- 共线 → 退化直线
	if n_len < 1e-9 then
		return M.distance(p1, p3)
	end

	n = vec_norm(n)

	-- 投影到圆平面
	local function proj(v)
		local d = vec_dot(v, n)
		return {
			v[1] - d * n[1],
			v[2] - d * n[2],
			v[3] - d * n[3],
		}
	end

	local P1 = proj(p1)
	local P2 = proj(p2)
	local P3 = proj(p3)

	-- 圆心计算
	local a = vec_sub(P2, P1)
	local b = vec_sub(P3, P1)

	local axb = vec_cross(a, b)
	local denom = 2 * vec_dot(axb, axb)

	if math.abs(denom) < 1e-12 then
		return M.distance(p1, p3)
	end

	local a2 = vec_dot(a, a)
	local b2 = vec_dot(b, b)

	local term1 = vec_cross(axb, a)
	local term2 = vec_cross(b, axb)

	local C = vec_add(P1, {
		(term1[1] * b2 + term2[1] * a2) / denom,
		(term1[2] * b2 + term2[2] * a2) / denom,
		(term1[3] * b2 + term2[3] * a2) / denom,
	})

	-- 半径
	local R = vec_len(vec_sub(P1, C))

	-- 角度
	local v1 = vec_sub(P1, C)
	local v2 = vec_sub(P3, C)

	local cos_theta = vec_dot(v1, v2) / (vec_len(v1) * vec_len(v2) + 1e-9)
	cos_theta = clamp(cos_theta, -1, 1)

	local theta = math.acos(cos_theta)

	-- 方向判断（顺/逆）
	local dir = vec_dot(vec_cross(v1, v2), n)
	if dir < 0 then
		theta = 2 * math.pi - theta
	end

	return R * theta
end

-- =========================================================
-- MoveC 中途停止长度估算
-- 输入:
--   p1,p2,p3: 原圆弧三点
--   pstop:    停止点
-- 返回:
--   估算从 p1 到 pstop 的弧长
-- 说明:
--   当前实现使用"弦长比例"估算，优先保证稳定性与抗异常能力。
-- =========================================================
function M.arcLengthPartial(p1, p2, p3, pstop)
	local total = M.arcLength(p1, p2, p3)
	if total == 0 then
		return 0
	end

	local d_total = M.distance(p1, p3)
	if d_total < 1e-9 then
		return 0
	end

	-- 用弦长比例估算进度（工程稳定方案）
	local t = M.distance(p1, pstop) / (d_total + 1e-9)
	t = clamp(t, 0, 1)

	return total * t
end

-- =========================================================
-- 回退内部工具（直线 + 圆弧）
-- =========================================================
local function pose_lerp(p1, p2, alpha)
	alpha = clamp(alpha or 0, 0, 1)
	local out = {}
	for i = 1, 6 do
		local a = p1[i] or 0
		local b = p2[i] or 0
		out[i] = a + (b - a) * alpha
	end
	return out
end

local function safe_angle_between(v1, v2)
	local d = vec_dot(v1, v2)
	local l = vec_len(v1) * vec_len(v2)
	if l < 1e-12 then
		return 0
	end
	return math.acos(clamp(d / l, -1, 1))
end

local function circle_from_3points(p1, p2, p3)
	local A = vec_sub(p2, p1)
	local B = vec_sub(p3, p1)
	local n = vec_cross(A, B)
	local n2 = vec_dot(n, n)
	if n2 < 1e-12 then
		return nil, "collinear"
	end

	local a2 = vec_dot(A, A)
	local b2 = vec_dot(B, B)
	local term1 = vec_cross(n, A)
	local term2 = vec_cross(B, n)
	local denom = 2 * n2

	local c = {
		p1[1] + (term1[1] * b2 + term2[1] * a2) / denom,
		p1[2] + (term1[2] * b2 + term2[2] * a2) / denom,
		p1[3] + (term1[3] * b2 + term2[3] * a2) / denom,
	}
	return {
		center = c,
		normal = vec_norm(n),
		radius = vec_len(vec_sub(p1, c)),
	}, nil
end

local function project_to_plane(pt, center, normal)
	local v = vec_sub(pt, center)
	local d = vec_dot(v, normal)
	return {
		pt[1] - d * normal[1],
		pt[2] - d * normal[2],
		pt[3] - d * normal[3],
	}
end

local function getLineRollbackPoint(stop_point, start_point, end_point, back_distance)
	if type(stop_point) ~= "table" or type(start_point) ~= "table" or type(end_point) ~= "table" then
		return false, {}, "invalid_input"
	end
	if #stop_point < 6 or #start_point < 6 or #end_point < 6 then
		return false, {}, "invalid_input"
	end
	if (back_distance or 0) < 0 then
		return false, {}, "invalid_input"
	end

	-- back_distance == 0 时直接返回 stop_point
	if (back_distance or 0) == 0 then
		return true, { stop_point[1], stop_point[2], stop_point[3], stop_point[4], stop_point[5], stop_point[6] }
	end

	local s = { start_point[1], start_point[2], start_point[3] }
	local e = { end_point[1], end_point[2], end_point[3] }
	local p = { stop_point[1], stop_point[2], stop_point[3] }
	local se = vec_sub(e, s)
	local se2 = vec_dot(se, se)
	if se2 < 1e-12 then
		return false, {}, "invalid_input"
	end

	-- 1) 将停止点投影到 start->end 线段上，得到垂足 foot。
	--    这样即便停止点来自摆动轨迹，也会回到主焊缝线进行回退计算。
	local t = clamp(vec_dot(vec_sub(p, s), se) / se2, 0, 1)
	local foot = { s[1] + se[1] * t, s[2] + se[2] * t, s[3] + se[3] * t }

	-- 2) 计算从起点到垂足的长度 dist_sf。
	local dist_sf = vec_len(vec_sub(foot, s))
	local seg_len = vec_len(se)
	if seg_len < 1e-12 then
		return false, {}, "invalid_input"
	end

	-- 3) 回退后距离起点的目标弧长（线长）:
	--    fallback_s = dist_sf - back_distance
	local fallback_s = dist_sf - (back_distance or 0)
	if fallback_s < 0 then
		-- 回退距离超过已走路径，视为非法回退
		return false, {}, "rollback_distance_exceeded"
	end
	if fallback_s == 0 then
		return true, { start_point[1], start_point[2], start_point[3], start_point[4], start_point[5], start_point[6] }
	end

	-- 4) 在 start->end 上按比例插值位置和姿态
	local alpha = clamp(fallback_s / seg_len, 0, 1)
	local pose = pose_lerp(start_point, end_point, alpha)
	return true, pose
end

local function getArcRollbackPoints(stop_point, start_point, middle_point, end_point, back_distance, num)
	-- 圆弧永远返回 2 个点：
	--   第1个点：回退点（back_distance > 0）或 stop_point 原样（back_distance == 0）
	--   第2个点：固定为前进方向中间点（stop 到弧终点的 1/2 处）
	if (back_distance or 0) < 0 then
		return false, {}, "invalid_input"
	end
	if #stop_point < 6 or #start_point < 6 or #middle_point < 6 or #end_point < 6 then
		return false, {}, "invalid_input"
	end

	local p1 = { start_point[1], start_point[2], start_point[3] }
	local p2 = { middle_point[1], middle_point[2], middle_point[3] }
	local p3 = { end_point[1], end_point[2], end_point[3] }
	local ps = { stop_point[1], stop_point[2], stop_point[3] }

	local circle = circle_from_3points(p1, p2, p3)
	if not circle then
		return false, {}, "invalid_input"
	end

	local c = circle.center
	local n = circle.normal
	local r = circle.radius
	if r < 1e-12 then
		return false, {}, "invalid_input"
	end

	local ps_proj = project_to_plane(ps, c, n)
	local v1 = vec_sub(p1, c)
	local v2 = vec_sub(p2, c)
	local v3 = vec_sub(p3, c)
	local vs = vec_sub(ps_proj, c)

	local a12 = safe_angle_between(v1, v2)
	local a23 = safe_angle_between(v2, v3)
	local a13 = a12 + a23

	local a1s = safe_angle_between(v1, vs)
	local choose_second = (a12 - a1s) <= 0
	if choose_second then
		a1s = a12 + safe_angle_between(v2, vs)
	end
	a1s = clamp(a1s, 0, a13)

	local ex = vec_norm(v1)
	local ey = vec_norm(vec_cross(n, ex))

	local result_points = {}

	-- ========== 第1个点：回退点（back_distance > 0）或 stop_point（back_distance == 0） ==========
	if (back_distance or 0) > 0 then
		local back_angle = (back_distance or 0) / r
		if back_angle > a1s then
			back_angle = a1s
		end
		local theta = a1s - back_angle
		local px = c[1] + r * (math.cos(theta) * ex[1] + math.sin(theta) * ey[1])
		local py = c[2] + r * (math.cos(theta) * ex[2] + math.sin(theta) * ey[2])
		local pz = c[3] + r * (math.cos(theta) * ex[3] + math.sin(theta) * ey[3])

		local pose
		if theta <= a12 and a12 > 1e-12 then
			pose = pose_lerp(start_point, middle_point, theta / a12)
		elseif a23 > 1e-12 then
			pose = pose_lerp(middle_point, end_point, (theta - a12) / a23)
		else
			pose = { end_point[1], end_point[2], end_point[3], end_point[4], end_point[5], end_point[6] }
		end

		pose[1], pose[2], pose[3] = px, py, pz
		table.insert(result_points, pose)
	else
		-- back_distance == 0，第1个点直接用 stop_point
		table.insert(
			result_points,
			{ stop_point[1], stop_point[2], stop_point[3], stop_point[4], stop_point[5], stop_point[6] }
		)
	end

	-- ========== 第2个点：前进方向中间点（stop点到圆弧终点一半位置） ==========
	local theta_direct_distance = a13 - a1s
	if theta_direct_distance < 1e-12 then
		-- 已在弧终点，第2个点也用终点
		table.insert(
			result_points,
			{ end_point[1], end_point[2], end_point[3], end_point[4], end_point[5], end_point[6] }
		)
	else
		local theta = a1s + theta_direct_distance / 2
		if theta > a13 then
			theta = a13
		end
		local px = c[1] + r * (math.cos(theta) * ex[1] + math.sin(theta) * ey[1])
		local py = c[2] + r * (math.cos(theta) * ex[2] + math.sin(theta) * ey[2])
		local pz = c[3] + r * (math.cos(theta) * ex[3] + math.sin(theta) * ey[3])

		local pose
		if theta <= a12 and a12 > 1e-12 then
			pose = pose_lerp(start_point, middle_point, theta / a12)
		elseif a23 > 1e-12 then
			pose = pose_lerp(middle_point, end_point, (theta - a12) / a23)
		else
			pose = { end_point[1], end_point[2], end_point[3], end_point[4], end_point[5], end_point[6] }
		end

		pose[1], pose[2], pose[3] = px, py, pz
		table.insert(result_points, pose)
	end

	return true, result_points
end

function M.getRollbackPoints(points, back_distance, num)
	-- 对外统一接口
	-- 输入:
	--   points:
	--     3 点 -> 直线回退 {stop, start, end}
	--     4 点 -> 圆弧回退 {stop, start, middle, end}
	--   back_distance: 回退距离（m），必须 >= 0
	--   num: 忽略（保留参数兼容）
	-- 输出:
	--   ok, rollback_points
	--   - ok=false: rollback_points={}
	--   - ok=true:
	--       直线: { pose1 }（1个点）
	--       圆弧: { pose1, pose2 }（2个点：回退点/stop_point(back_distance = 0) + 前进方向中间点）

	-- ========== 打印传入点 ==========
	local log_pfx = welding_common.log_prefix or ""
	if type(points) == "table" and #points == 3 then
		log_info(
			log_pfx
				.. "getRollbackPoints INPUT [Line]: "
				.. "stop="
				.. formatPose(points[1])
				.. ", start="
				.. formatPose(points[2])
				.. ", end="
				.. formatPose(points[3])
				.. ", back_distance="
				.. tostring(back_distance)
		)
	elseif type(points) == "table" and #points == 4 then
		log_info(
			log_pfx
				.. "getRollbackPoints INPUT [Arc]: "
				.. "stop="
				.. formatPose(points[1])
				.. ", start="
				.. formatPose(points[2])
				.. ", middle="
				.. formatPose(points[3])
				.. ", end="
				.. formatPose(points[4])
				.. ", back_distance="
				.. tostring(back_distance)
		)
	else
		log_info(
			log_pfx .. "getRollbackPoints INPUT: invalid points type/count, back_distance=" .. tostring(back_distance)
		)
	end

	if type(points) ~= "table" or #points == 0 then
		log_info(log_pfx .. "getRollbackPoints OUTPUT: ok=false, err=invalid_input")
		return false, {}, "invalid_input"
	end
	if (back_distance or 0) < 0 then
		log_info(log_pfx .. "getRollbackPoints OUTPUT: ok=false, err=invalid_input")
		return false, {}, "invalid_input"
	end
	num = num or 1
	if num < 1 then
		log_info(log_pfx .. "getRollbackPoints OUTPUT: ok=false, err=invalid_input")
		return false, {}, "invalid_input"
	end

	if #points == 3 then
		local ok, p, err = getLineRollbackPoint(points[1], points[2], points[3], back_distance)
		if not ok then
			log_info(log_pfx .. "getRollbackPoints OUTPUT [Line]: ok=false, err=" .. tostring(err))
			return false, {}, err
		end
		log_info(log_pfx .. "getRollbackPoints OUTPUT [Line]: ok=true, rollback=" .. formatPose(p))
		return true, { p }, nil
	elseif #points == 4 then
		local ok, pts, err = getArcRollbackPoints(points[1], points[2], points[3], points[4], back_distance, num)
		if not ok then
			log_info(log_pfx .. "getRollbackPoints OUTPUT [Arc]: ok=false, err=" .. tostring(err))
			return false, {}, err
		end
		local out_log = "ok=true"
		for i = 1, #pts do
			out_log = out_log .. ", point" .. i .. "=" .. formatPose(pts[i])
		end
		log_info(log_pfx .. "getRollbackPoints OUTPUT [Arc]: " .. out_log)
		return ok, pts, err
	end
	log_info(log_pfx .. "getRollbackPoints OUTPUT: ok=false, err=invalid_input")
	return false, {}, "invalid_input"
end

return M
