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 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: "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);
    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;
});
