const fs = require("fs");
const path = require("path");

const rootDir = path.resolve(__dirname, "..");
const defaultModulePath = path.join(rootDir, "public", "wasm", "smart_math.js");

function readJson(relativePath) {
  const filePath = path.join(rootDir, relativePath);
  return JSON.parse(fs.readFileSync(filePath, "utf8"));
}

function ensure(condition, message) {
  if (!condition) {
    throw new Error(message);
  }
}

function getByPath(value, pathExpression) {
  const parts = pathExpression.split(".");
  let current = value;

  for (const part of parts) {
    if (current === undefined || current === null) {
      return undefined;
    }

    if (/^\d+$/.test(part)) {
      current = current[Number(part)];
    } else {
      current = current[part];
    }
  }

  return current;
}

function jointArrayToString(joints) {
  return joints.join(",");
}

function poseToString(position, orientation) {
  return [...position, ...orientation].join(",");
}

function positionDistance(positionA, positionB) {
  const dx = positionA[0] - positionB[0];
  const dy = positionA[1] - positionB[1];
  const dz = positionA[2] - positionB[2];
  return Math.sqrt(dx * dx + dy * dy + dz * dz);
}

function quaternionDistance(q1, q2) {
  const dot = Math.abs(
    q1[0] * q2[0] +
      q1[1] * q2[1] +
      q1[2] * q2[2] +
      q1[3] * q2[3]
  );
  return 1 - Math.min(1, dot);
}

async function loadModule(modulePath) {
  const required = require(modulePath);

  if (typeof required === "function") {
    return required({
      noInitialRun: true,
      locateFile: (fileName) => path.join(path.dirname(modulePath), fileName),
    });
  }

  if (required && typeof required === "object") {
    return required;
  }

  throw new Error(`Unsupported WASM loader type: ${typeof required}`);
}

function allocString(module, value) {
  const size = module.lengthBytesUTF8(value) + 1;
  const ptr = module._malloc(size);
  module.stringToUTF8(value, ptr, size);
  return ptr;
}

function callJsonFunction(module, exportName, payload) {
  const requestText = typeof payload === "string" ? payload : JSON.stringify(payload);
  const requestPtr = allocString(module, requestText);
  let responsePtr = 0;

  try {
    responsePtr = module[exportName](requestPtr);
    const responseText = module.UTF8ToString(responsePtr);
    return JSON.parse(responseText);
  } finally {
    if (responsePtr) {
      module._smart_free_string(responsePtr);
    }
    module._free(requestPtr);
  }
}

function callNoArgJsonFunction(module, exportName) {
  const responsePtr = module[exportName]();
  try {
    const responseText = module.UTF8ToString(responsePtr);
    return JSON.parse(responseText);
  } finally {
    module._smart_free_string(responsePtr);
  }
}

function callBusinessApi(module, request) {
  return callJsonFunction(module, "_func", request);
}

function buildForwardRequest(robotUuid, joints) {
  return {
    msg: "fk roundtrip",
    req_code: "AUTO_FK",
    req_from: "wasm_test",
    req_cmd: "Cmd_Kinematics_forward_pose_str",
    req_param: {
      robot_uuid: robotUuid,
      q_init_str: jointArrayToString(joints),
    },
  };
}

function buildInverseRequest(command, robotUuid, poseStr, qInitStr, extra = {}) {
  return {
    msg: "ik roundtrip",
    req_code: "AUTO_IK",
    req_from: "wasm_test",
    req_cmd: command,
    req_param: {
      robot_uuid: robotUuid,
      pose_str: poseStr,
      q_init_str: qInitStr,
      ...extra,
    },
  };
}

function buildSingularityRequest(robotUuid, joints) {
  return {
    msg: "singularity check",
    req_code: "AUTO_SINGULARITY",
    req_from: "wasm_test",
    req_cmd: "Cmd_Kinematics_check_singularity",
    req_param: {
      robot_uuid: robotUuid,
      joints_str: jointArrayToString(joints),
    },
  };
}

// 构造 link 名称被整体替换的 URDF，用于验证初始化能自动推导运动学链。
function buildRenamedUrdfFromDocs() {
  const urdfPath = path.join(rootDir, "docs", "urdf.xml");
  return fs
    .readFileSync(urdfPath, "utf8")
    .replaceAll("base_link-base", "renamed_root_link-renamed_world")
    .replaceAll("base_link", "renamed_root_link")
    .replaceAll("tool0", "renamed_tcp")
    .replaceAll('link name="base"', 'link name="renamed_world"')
    .replaceAll('parent link="base"', 'parent link="renamed_world"');
}

// 注册改名后的 URDF 并执行一次 FK，确认底层不再依赖固定 base/tool0。
async function runRenamedUrdfChainCase(module) {
  const robotUuid = "renamed_chain_robot";
  const urdfBase64 = Buffer.from(buildRenamedUrdfFromDocs(), "utf8").toString("base64");

  const initResponse = callBusinessApi(module, {
    msg: "init renamed urdf",
    req_code: "AUTO_RENAMED_INIT",
    req_from: "wasm_test",
    req_cmd: "Cmd_InitRobot",
    req_param: {
      robot_uuid: robotUuid,
      urdf_base64: urdfBase64,
      force_update: true,
    },
  });

  ensure(initResponse.success === true, "renamed URDF init request failed");
  ensure(getByPath(initResponse, "res_data.success") === true, "renamed URDF init result is not successful");

  const forwardResponse = callBusinessApi(module, buildForwardRequest(robotUuid, [0, 0, 0, 0, 0, 0]));
  ensure(forwardResponse.success === true, "renamed URDF forward request failed");
  ensure(getByPath(forwardResponse, "res_data.success") === true, "renamed URDF forward result is not successful");
  ensure(Array.isArray(getByPath(forwardResponse, "res_data.position")), "renamed URDF forward position is missing");

  return {
    details: {
      position: getByPath(forwardResponse, "res_data.position"),
      orientation: getByPath(forwardResponse, "res_data.orientation"),
    },
  };
}

// 发送超过 URDF 关节上下限的 FK 请求，确认底层会拒绝计算。
async function runJointLimitCase(module) {
  const response = callBusinessApi(module, buildForwardRequest("abb_irb120_3_58", [7, 0, 0, 0, 0, 0]));
  ensure(response.success === false, "joint limit case should fail at response level");
  ensure(String(getByPath(response, "res_data.error")).includes("Forward kinematics calculation failed"), "joint limit error is missing");
  return response;
}

// 检查奇异点分析接口会返回完整的雅可比 SVD 指标。
async function runSingularityCheckCase(module) {
  const response = callBusinessApi(module, buildSingularityRequest("abb_irb120_3_58", [0.15, -0.25, 0.35, 0.1, -0.2, 0.3]));
  ensure(response.success === true, "singularity check request failed");
  ensure(getByPath(response, "res_data.success") === true, "singularity check result is not successful");
  ensure(Array.isArray(getByPath(response, "res_data.singular_values")), "singular values are missing");
  ensure(getByPath(response, "res_data.singular_values").length === 6, "singular value count should be 6");
  ensure(typeof getByPath(response, "res_data.condition_number") === "number", "condition number is missing");
  ensure(typeof getByPath(response, "res_data.risk_level") === "string", "risk level is missing");
  return {
    details: {
      riskLevel: getByPath(response, "res_data.risk_level"),
      rank: getByPath(response, "res_data.rank"),
      minSingularValue: getByPath(response, "res_data.min_singular_value"),
      conditionNumber: getByPath(response, "res_data.condition_number"),
    },
  };
}

function assertStaticCase(response, assertions) {
  for (const assertion of assertions) {
    const actual = getByPath(response, assertion.path);

    if (assertion.equals !== undefined) {
      ensure(
        actual === assertion.equals,
        `${assertion.path} expected ${JSON.stringify(assertion.equals)}, got ${JSON.stringify(actual)}`
      );
    }

    if (assertion.exists) {
      ensure(actual !== undefined && actual !== null, `${assertion.path} is missing`);
    }

    if (assertion.gte !== undefined) {
      ensure(actual >= assertion.gte, `${assertion.path} expected >= ${assertion.gte}, got ${actual}`);
    }

    if (assertion.lengthEquals !== undefined) {
      ensure(Array.isArray(actual), `${assertion.path} is not an array`);
      ensure(
        actual.length === assertion.lengthEquals,
        `${assertion.path} expected length ${assertion.lengthEquals}, got ${actual.length}`
      );
    }

    if (assertion.lengthGte !== undefined) {
      ensure(Array.isArray(actual), `${assertion.path} is not an array`);
      ensure(
        actual.length >= assertion.lengthGte,
        `${assertion.path} expected length >= ${assertion.lengthGte}, got ${actual.length}`
      );
    }

    if (assertion.includes !== undefined) {
      ensure(
        String(actual).includes(assertion.includes),
        `${assertion.path} expected to include ${assertion.includes}, got ${actual}`
      );
    }
  }
}

async function runStaticCase(module, testCase) {
  const request = readJson(testCase.requestFile);
  const response = callBusinessApi(module, request);
  assertStaticCase(response, testCase.assertions);
  return response;
}

async function runRoundtripSingleCase(module, testCase) {
  const seed = readJson(testCase.seedFile);
  const forwardResponse = callBusinessApi(module, buildForwardRequest(seed.robot_uuid, seed.target_joints));

  ensure(forwardResponse.success === true, "forward request failed");
  ensure(getByPath(forwardResponse, "res_data.success") === true, "forward result is not successful");

  const forwardPosition = getByPath(forwardResponse, "res_data.position");
  const forwardOrientation = getByPath(forwardResponse, "res_data.orientation");
  const poseStr = poseToString(forwardPosition, forwardOrientation);

  const inverseResponse = callBusinessApi(
    module,
    buildInverseRequest(
      "Cmd_Kinematics_inverse_pose_str_NoDifference",
      seed.robot_uuid,
      poseStr,
      seed.q_init_str
    )
  );

  ensure(inverseResponse.success === true, "inverse request failed");
  ensure(getByPath(inverseResponse, "res_data.success") === true, "inverse result is not successful");

  const jointSolutions = getByPath(inverseResponse, "res_data.joints");
  ensure(Array.isArray(jointSolutions) && jointSolutions.length >= 1, "inverse result contains no joint solution");

  const finalJoints = jointSolutions[jointSolutions.length - 1];
  const forwardCheck = callBusinessApi(module, buildForwardRequest(seed.robot_uuid, finalJoints));

  ensure(forwardCheck.success === true, "forward validation request failed");
  ensure(getByPath(forwardCheck, "res_data.success") === true, "forward validation result is not successful");

  const checkedPosition = getByPath(forwardCheck, "res_data.position");
  const checkedOrientation = getByPath(forwardCheck, "res_data.orientation");

  const posDiff = positionDistance(forwardPosition, checkedPosition);
  const quatDiff = quaternionDistance(forwardOrientation, checkedOrientation);

  ensure(
    posDiff <= seed.position_tolerance,
    `roundtrip position diff ${posDiff} exceeds tolerance ${seed.position_tolerance}`
  );
  ensure(
    quatDiff <= seed.orientation_tolerance,
    `roundtrip quaternion diff ${quatDiff} exceeds tolerance ${seed.orientation_tolerance}`
  );

  return {
    response: inverseResponse,
    details: {
      positionDiff: posDiff,
      quaternionDiff: quatDiff,
      solutionCount: jointSolutions.length,
    },
  };
}

async function runRoundtripPathCase(module, testCase) {
  const seed = readJson(testCase.seedFile);

  const startForward = callBusinessApi(module, buildForwardRequest(seed.robot_uuid, seed.start_joints));
  const endForward = callBusinessApi(module, buildForwardRequest(seed.robot_uuid, seed.end_joints));

  const startPose = poseToString(
    getByPath(startForward, "res_data.position"),
    getByPath(startForward, "res_data.orientation")
  );
  const endPose = poseToString(
    getByPath(endForward, "res_data.position"),
    getByPath(endForward, "res_data.orientation")
  );

  const inverseResponse = callBusinessApi(
    module,
    buildInverseRequest(
      "Cmd_Kinematics_inverse_pose_str_2PSteps",
      seed.robot_uuid,
      `${startPose};${endPose}`,
      seed.q_init_str,
      { steps_str: String(seed.steps) }
    )
  );

  ensure(inverseResponse.success === true, "path inverse request failed");
  ensure(getByPath(inverseResponse, "res_data.success") === true, "path inverse result is not successful");

  const jointSolutions = getByPath(inverseResponse, "res_data.joints");
  ensure(Array.isArray(jointSolutions), "path inverse result is not an array");
  ensure(
    jointSolutions.length === seed.steps,
    `path inverse expected ${seed.steps} solutions, got ${jointSolutions.length}`
  );

  const firstForwardCheck = callBusinessApi(module, buildForwardRequest(seed.robot_uuid, jointSolutions[0]));
  const lastForwardCheck = callBusinessApi(
    module,
    buildForwardRequest(seed.robot_uuid, jointSolutions[jointSolutions.length - 1])
  );

  const startPosDiff = positionDistance(
    getByPath(startForward, "res_data.position"),
    getByPath(firstForwardCheck, "res_data.position")
  );
  const endPosDiff = positionDistance(
    getByPath(endForward, "res_data.position"),
    getByPath(lastForwardCheck, "res_data.position")
  );

  ensure(
    startPosDiff <= seed.position_tolerance,
    `path start position diff ${startPosDiff} exceeds tolerance ${seed.position_tolerance}`
  );
  ensure(
    endPosDiff <= seed.position_tolerance,
    `path end position diff ${endPosDiff} exceeds tolerance ${seed.position_tolerance}`
  );

  return {
    response: inverseResponse,
    details: {
      solutionCount: jointSolutions.length,
      startPositionDiff: startPosDiff,
      endPositionDiff: endPosDiff,
    },
  };
}

const suite = [
  {
    id: "list_robots_after_init",
    type: "static",
    requestFile: "tests/testdata/kinematics/list_robots.json",
    assertions: [
      { path: "success", equals: true },
      { path: "res_data.success", equals: true },
      { path: "res_data.count", gte: 2 },
    ],
  },
  {
    id: "kinematics_forward_zero",
    type: "static",
    requestFile: "tests/testdata/kinematics/forward_zero.json",
    assertions: [
      { path: "success", equals: true },
      { path: "res_data.success", equals: true },
      { path: "res_data.position", lengthEquals: 3 },
      { path: "res_data.orientation", lengthEquals: 4 },
    ],
  },
  {
    id: "kinematics_forward_all_joints_path",
    type: "static",
    requestFile: "tests/testdata/kinematics/forward_all_joints_path.json",
    assertions: [
      { path: "success", equals: true },
      { path: "res_data.success", equals: true },
      { path: "res_data.OPERATION.OPERATION.frames", lengthEquals: 2 },
    ],
  },
  {
    id: "kinematics_renamed_urdf_chain",
    type: "renamed_urdf_chain",
  },
  {
    id: "kinematics_joint_limit_rejects_fk",
    type: "joint_limit",
  },
  {
    id: "kinematics_singularity_check",
    type: "singularity_check",
  },
  {
    id: "spc_basic_5x30",
    type: "static",
    requestFile: "tests/testdata/spc/basic_5x30.json",
    assertions: [
      { path: "success", equals: true },
      { path: "res_data.XR.n", equals: 5 },
      { path: "res_data.XR.k", equals: 30 },
      { path: "res_data.Cpk.USL", equals: 1.7 },
      { path: "res_data.Cpk.LSL", equals: 1.5 },
      { path: "res_data.Cpk.Cpk", gte: 0.1 },
    ],
  },
  {
    id: "spc_invalid_count",
    type: "static",
    requestFile: "tests/testdata/spc/invalid_count.json",
    assertions: [
      { path: "success", equals: false },
      { path: "code", equals: 1000 },
      { path: "res_data.error", includes: "SPC calculation failed" },
      { path: "res_data.error", includes: "数据个数不匹配" },
    ],
  },
  {
    id: "fourbar_valid",
    type: "static",
    requestFile: "tests/testdata/fourbar/crank_slider_valid.json",
    assertions: [
      { path: "success", equals: true },
      { path: "res_data.success", equals: true },
      { path: "res_data.points.B.x", exists: true },
      { path: "res_data.trajectory", lengthGte: 1 },
      { path: "res_data.slider_trajectory", lengthGte: 1 },
    ],
  },
  {
    id: "fourbar_invalid",
    type: "static",
    requestFile: "tests/testdata/fourbar/crank_slider_invalid.json",
    assertions: [
      { path: "success", equals: false },
      { path: "code", equals: 1000 },
      { path: "res_data.success", equals: false },
      { path: "res_data.error", includes: "Invalid parameters" },
      { path: "res_data.validation_errors", lengthGte: 1 },
    ],
  },
  {
    id: "fourbar_simulate",
    type: "static",
    requestFile: "tests/testdata/fourbar/crank_slider_simulate.json",
    assertions: [
      { path: "success", equals: true },
      { path: "res_data.success", equals: true },
      { path: "res_data.frame_count", equals: 5 },
      { path: "res_data.frames", lengthEquals: 5 },
      { path: "res_data.frames.0.angleDeg", equals: 0 },
      { path: "res_data.frames.4.angleDeg", equals: 180 },
      { path: "res_data.frames.0.points.B.x", exists: true },
      { path: "res_data.frames.0.poses.TCP.tx", exists: true },
      { path: "res_data.trajectory", lengthEquals: 5 },
      { path: "res_data.slider_trajectory", lengthEquals: 5 },
    ],
  },
  {
    id: "fourbar_unified_rrrp",
    type: "static",
    requestFile: "tests/testdata/fourbar/simulate_rrrp.json",
    assertions: [
      { path: "success", equals: true },
      { path: "res_data.success", equals: true },
      { path: "res_data.mechanismType", equals: "RRRP" },
      { path: "res_data.frame_count", equals: 5 },
      { path: "res_data.frames", lengthEquals: 5 },
      { path: "res_data.frames.0.points.B.x", exists: true },
      { path: "res_data.frames.0.points.S.x", exists: true },
      { path: "res_data.trajectory", lengthEquals: 5 },
      { path: "res_data.slider_trajectory", lengthEquals: 5 },
    ],
  },
  {
    id: "fourbar_unified_prrr",
    type: "static",
    requestFile: "tests/testdata/fourbar/simulate_prrr.json",
    assertions: [
      { path: "success", equals: true },
      { path: "res_data.success", equals: true },
      { path: "res_data.mechanismType", equals: "PRRR" },
      { path: "res_data.inputName", equals: "sliderX" },
      { path: "res_data.frame_count", equals: 5 },
      { path: "res_data.frames.0.points.B.x", exists: true },
      { path: "res_data.frames.0.points.S.x", exists: true },
      { path: "res_data.trajectory", lengthEquals: 5 },
      { path: "res_data.slider_trajectory", lengthEquals: 5 },
      { path: "res_data.alternative_trajectory", lengthEquals: 5 },
    ],
  },
  {
    id: "fourbar_unified_rprr",
    type: "static",
    requestFile: "tests/testdata/fourbar/simulate_rprr.json",
    assertions: [
      { path: "success", equals: true },
      { path: "res_data.success", equals: true },
      { path: "res_data.mechanismType", equals: "RPRR" },
      { path: "res_data.frame_count", equals: 5 },
      { path: "res_data.frames.0.points.O.x", exists: true },
      { path: "res_data.frames.0.points.B.x", exists: true },
      { path: "res_data.trajectory", lengthEquals: 5 },
      { path: "res_data.crank_circle", lengthEquals: 5 },
      { path: "res_data.rocker_trajectory", lengthEquals: 5 },
    ],
  },
  {
    id: "fourbar_unified_rrrr",
    type: "static",
    requestFile: "tests/testdata/fourbar/simulate_rrrr.json",
    assertions: [
      { path: "success", equals: true },
      { path: "res_data.success", equals: true },
      { path: "res_data.mechanismType", equals: "RRRR" },
      { path: "res_data.frame_count", equals: 5 },
      { path: "res_data.frames.0.points.A.x", exists: true },
      { path: "res_data.frames.0.points.D.x", exists: true },
      { path: "res_data.trajectory", lengthEquals: 5 },
      { path: "res_data.trajectory_c", lengthEquals: 5 },
      { path: "res_data.crank_circle", lengthEquals: 5 },
    ],
  },
  {
    id: "quadruped_points_from_motor_angles",
    type: "static",
    requestFile: "tests/testdata/quadruped/calculate_points_from_motor_angles.json",
    assertions: [
      { path: "success", equals: true },
      { path: "res_data.objStates", lengthGte: 20 },
    ],
  },
  {
    id: "quadruped_forward_gait",
    type: "static",
    requestFile: "tests/testdata/quadruped/perform_forward_kinematics.json",
    assertions: [
      { path: "success", equals: true },
      { path: "res_data.GaitInfo.TotalFrames", equals: 40 },
      { path: "res_data.ConstraintData.AB1_AB_Constraint.IsValid", equals: true },
    ],
  },
  {
    id: "ik_roundtrip_single_pose",
    type: "roundtrip_single",
    seedFile: "tests/testdata/kinematics/roundtrip_single_seed.json",
  },
  {
    id: "ik_roundtrip_two_pose_path",
    type: "roundtrip_path",
    seedFile: "tests/testdata/kinematics/roundtrip_path_seed.json",
  },
];

async function runCase(module, testCase) {
  switch (testCase.type) {
    case "static":
      return runStaticCase(module, testCase);
    case "roundtrip_single":
      return runRoundtripSingleCase(module, testCase);
    case "roundtrip_path":
      return runRoundtripPathCase(module, testCase);
    case "renamed_urdf_chain":
      return runRenamedUrdfChainCase(module, testCase);
    case "joint_limit":
      return runJointLimitCase(module, testCase);
    case "singularity_check":
      return runSingularityCheckCase(module, testCase);
    default:
      throw new Error(`Unknown test type: ${testCase.type}`);
  }
}

async function main() {
  const modulePath = path.resolve(process.argv[2] || defaultModulePath);
  ensure(fs.existsSync(modulePath), `WASM JS loader not found: ${modulePath}`);

  console.log(`Loading module: ${modulePath}`);
  const module = await loadModule(modulePath);

  ensure(typeof module._init_func === "function", "_init_func is not exported");
  ensure(typeof module._func === "function", "_func is not exported");
  ensure(typeof module.lengthBytesUTF8 === "function", "lengthBytesUTF8 is not exported");
  ensure(typeof module.stringToUTF8 === "function", "stringToUTF8 is not exported");
  ensure(typeof module.UTF8ToString === "function", "UTF8ToString is not exported");

  const initResponse = callNoArgJsonFunction(module, "_init_func");
  ensure(initResponse.success === true, `init_func failed: ${JSON.stringify(initResponse)}`);

  console.log("Initialization complete.");

  let passed = 0;
  const failures = [];

  for (const testCase of suite) {
    try {
      const result = await runCase(module, testCase);
      passed += 1;

      if (result && result.details) {
        console.log(`[PASS] ${testCase.id} ${JSON.stringify(result.details)}`);
      } else {
        console.log(`[PASS] ${testCase.id}`);
      }
    } catch (error) {
      failures.push({ id: testCase.id, message: error.message });
      console.error(`[FAIL] ${testCase.id}: ${error.message}`);
    }
  }

  console.log("");
  console.log(`Result: ${passed}/${suite.length} passed`);

  if (failures.length > 0) {
    console.log("Failed cases:");
    for (const failure of failures) {
      console.log(`- ${failure.id}: ${failure.message}`);
    }
    process.exitCode = 1;
  }
}

main().catch((error) => {
  console.error(error);
  process.exitCode = 1;
});