import { KdlWorkerRuntime } from "./runtime.js";
import type { PerformanceBaselineResult, PerformanceMetric, Pose, TrajectoryResult } from "./types.js";

const SIX_AXIS_URDF = `
<robot name="performance_6_axis">
  <link name="base_link"/>
  <link name="link_1"/>
  <link name="link_2"/>
  <link name="link_3"/>
  <link name="link_4"/>
  <link name="link_5"/>
  <link name="tool0"/>
  <joint name="joint_1" type="revolute">
    <parent link="base_link"/>
    <child link="link_1"/>
    <origin xyz="0 0 0.1" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5" acceleration="5"/>
  </joint>
  <joint name="joint_2" type="revolute">
    <parent link="link_1"/>
    <child link="link_2"/>
    <origin xyz="0.2 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5" acceleration="5"/>
  </joint>
  <joint name="joint_3" type="revolute">
    <parent link="link_2"/>
    <child link="link_3"/>
    <origin xyz="0.2 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5" acceleration="5"/>
  </joint>
  <joint name="joint_4" type="revolute">
    <parent link="link_3"/>
    <child link="link_4"/>
    <origin xyz="0.1 0 0.1" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5" acceleration="5"/>
  </joint>
  <joint name="joint_5" type="revolute">
    <parent link="link_4"/>
    <child link="link_5"/>
    <origin xyz="0.1 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5" acceleration="5"/>
  </joint>
  <joint name="joint_6" type="revolute">
    <parent link="link_5"/>
    <child link="tool0"/>
    <origin xyz="0.1 0 0" rpy="0 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5" acceleration="5"/>
  </joint>
</robot>
`;

const PLANAR_URDF = `
<robot name="performance_planar">
  <link name="base_link"/>
  <link name="link_1"/>
  <link name="tool0"/>
  <joint name="joint_1" type="revolute">
    <parent link="base_link"/>
    <child link="link_1"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.141592653589793" upper="3.141592653589793" velocity="2.5" acceleration="5"/>
  </joint>
  <joint name="joint_2" type="prismatic">
    <parent link="link_1"/>
    <child link="tool0"/>
    <axis xyz="1 0 0"/>
    <limit lower="0" upper="1" velocity="1" acceleration="2"/>
  </joint>
</robot>
`;

export interface PerformanceBaselineOptions {
  fkIterations?: number;
  ikIterations?: number;
  reachabilityTargets?: number;
  sampleTime?: number;
  trajectorySeconds?: number;
}

export async function runPerformanceBaseline(
  options: PerformanceBaselineOptions = {}
): Promise<PerformanceBaselineResult> {
  const fkIterations = options.fkIterations ?? 1_000;
  const ikIterations = options.ikIterations ?? 200;
  const reachabilityTargets = options.reachabilityTargets ?? 1_000;
  const sampleTime = options.sampleTime ?? 0.004;
  const trajectorySeconds = options.trajectorySeconds ?? 10;
  const runtime = new KdlWorkerRuntime();
  const metrics: PerformanceMetric[] = [];

  const initStart = performance.now();
  await runtime.init({ wasmBuild: "performance-baseline" });
  const sixAxisHandle = await runtime.loadRobotFromUrdf(SIX_AXIS_URDF, {
    robotId: "performance_6_axis",
    baseLink: "base_link",
    tipLink: "tool0"
  });
  metrics.push(singleMetric("robot_init_6_axis", performance.now() - initStart, 1_000));

  const fkInput = new Float64Array([0.1, -0.2, 0.15, 0.05, -0.1, 0.2]);
  const fkOutput = new Float64Array(7);
  metrics.push(await repeatedMetric("fk_pose7_typed_array", fkIterations, 1, () => {
    return runtime.fkPose7(sixAxisHandle, fkInput, fkOutput);
  }));

  const planarHandle = await runtime.loadRobotFromUrdf(PLANAR_URDF, {
    robotId: "performance_planar",
    baseLink: "base_link",
    tipLink: "tool0"
  });

  const ikTargets = makeTargets(ikIterations, 0.15, 0.65);
  metrics.push(await repeatedMetric("ik_planar_average", ikIterations, 10, (index) => {
    return runtime.ik(planarHandle, new Float64Array([0, 0.2]), ikTargets[index]!, {
      positionTolerance: 1e-9
    });
  }));

  const reachability = makePoseTargets(reachabilityTargets, 0.05, 0.95);
  const reachabilityStart = performance.now();
  const reachabilityResult = await runtime.checkReachabilityBatch(planarHandle, reachability);
  const reachabilityMs = performance.now() - reachabilityStart;
  metrics.push({
    name: "reachability_batch_1000",
    iterations: 1,
    totalMs: reachabilityMs,
    averageMs: reachabilityMs,
    thresholdMs: 500,
    points: reachabilityTargets,
    ok: reachabilityMs <= 500 && reachabilityResult.length === reachabilityTargets
  });

  const trajectoryStart = performance.now();
  const trajectory = await runtime.planMoveJ(planarHandle, {
    startJoints: [0, 0],
    target: {
      id: "ten_second_goal",
      joints: [0, 1]
    },
    speed: {
      kind: "joint_abs",
      velocity: 1 / trajectorySeconds,
      acceleration: 1
    },
    zone: {
      kind: "fine"
    },
    sampleTime
  });
  const trajectoryMs = performance.now() - trajectoryStart;
  metrics.push(trajectoryMetric("trajectory_10s_4ms", trajectory, trajectoryMs, 500));

  await runtime.dispose();
  return {
    ok: metrics.every((metric) => metric.ok),
    metrics,
    diagnostics: metrics.flatMap((metric) =>
      metric.ok
        ? []
        : [
            {
              severity: "warning" as const,
              code: "KDL_PERFORMANCE_BASELINE_MISS",
              message: `${metric.name} exceeded ${metric.thresholdMs ?? "unbounded"} ms`,
              data: {
                metric
              }
            }
          ]
    )
  };
}

function singleMetric(name: string, totalMs: number, thresholdMs: number): PerformanceMetric {
  return {
    name,
    iterations: 1,
    totalMs,
    averageMs: totalMs,
    thresholdMs,
    ok: totalMs <= thresholdMs
  };
}

async function repeatedMetric(
  name: string,
  iterations: number,
  thresholdMs: number,
  fn: (index: number) => unknown | Promise<unknown>
): Promise<PerformanceMetric> {
  let maxMs = 0;
  const start = performance.now();
  for (let index = 0; index < iterations; index += 1) {
    const before = performance.now();
    await fn(index);
    maxMs = Math.max(maxMs, performance.now() - before);
  }
  const totalMs = performance.now() - start;
  const averageMs = totalMs / iterations;
  return {
    name,
    iterations,
    totalMs,
    averageMs,
    maxMs,
    thresholdMs,
    ok: averageMs <= thresholdMs
  };
}

function trajectoryMetric(
  name: string,
  trajectory: TrajectoryResult,
  totalMs: number,
  thresholdMs: number
): PerformanceMetric {
  const points = trajectory.points.length;
  return {
    name,
    iterations: 1,
    totalMs,
    averageMs: totalMs,
    thresholdMs,
    points,
    ok: trajectory.ok && points >= 2_500 && totalMs <= thresholdMs
  };
}

function makeTargets(count: number, minRadius: number, maxRadius: number): Pose[] {
  return Array.from({ length: count }, (_, index) => {
    const ratio = count <= 1 ? 0 : index / (count - 1);
    const angle = ratio * Math.PI * 2;
    const radius = minRadius + (maxRadius - minRadius) * ((index % 97) / 96);
    return pose(Math.cos(angle) * radius, Math.sin(angle) * radius);
  });
}

function makePoseTargets(count: number, minRadius: number, maxRadius: number) {
  return makeTargets(count, minRadius, maxRadius).map((poseValue, index) => ({
    id: `target_${index}`,
    pose: poseValue
  }));
}

function pose(x: number, y: number): Pose {
  return {
    position: [x, y, 0],
    quaternion: [0, 0, 0, 1]
  };
}
