import { KdlStructuredError } from "./rpc.js";
import type {
  CycleTimeResult,
  MotionDiagnostic,
  PathPlanResult,
  Pose,
  TrajectoryPoint,
  TrajectoryResult
} from "./types.js";

export function estimateCycleTime(input: TrajectoryResult | PathPlanResult): CycleTimeResult {
  const diagnostics: MotionDiagnostic[] = input.diagnostics ?? [];
  if (isPathPlanResult(input)) {
    const segmentTimes = input.segments.map((segment) =>
      cycleTimeSegment(segment.motion, segment.duration, segment.points[0]?.segmentId)
    );
    return {
      ok: input.ok,
      motionTime: input.duration,
      totalTime: input.duration,
      segmentTimes,
      diagnostics
    };
  }

  return {
    ok: input.ok,
    motionTime: input.duration,
    totalTime: input.duration,
    segmentTimes: [cycleTimeSegment(input.motion, input.duration, input.points[0]?.segmentId)],
    diagnostics
  };
}

function cycleTimeSegment(
  motion: TrajectoryResult["motion"],
  duration: number,
  segmentId?: string
): CycleTimeResult["segmentTimes"][number] {
  return {
    motion,
    duration,
    ...(segmentId ? { segmentId } : {})
  };
}

export function resampleTrajectory(trajectory: TrajectoryResult, sampleTime: number): TrajectoryResult {
  if (!Number.isFinite(sampleTime) || sampleTime <= 0) {
    throw new KdlStructuredError("KDL_INVALID_SAMPLE_TIME", "sampleTime must be a finite positive number");
  }
  if (trajectory.points.length === 0) {
    return {
      ...trajectory,
      sampleTime,
      diagnostics: [
        ...trajectory.diagnostics,
        {
          severity: "warning",
          code: "KDL_RESAMPLE_EMPTY_TRAJECTORY",
          message: "Cannot resample a trajectory without points"
        }
      ]
    };
  }

  const duration = trajectory.duration;
  const times = duration === 0 ? [0] : sampleTimes(duration, sampleTime);
  const points = times.map((time, index) => {
    const source = interpolatePoint(trajectory.points, time);
    const previous = index > 0 ? times[index - 1]! : time;
    return {
      ...source,
      index,
      time,
      dt: index === 0 ? 0 : time - previous
    };
  });

  return {
    ...trajectory,
    sampleTime,
    points,
    diagnostics: [
      ...trajectory.diagnostics,
      {
        severity: "info",
        code: "KDL_TRAJECTORY_RESAMPLED",
        message: `Trajectory was resampled to ${sampleTime}s`
      }
    ]
  };
}

function isPathPlanResult(input: TrajectoryResult | PathPlanResult): input is PathPlanResult {
  return "segments" in input;
}

function sampleTimes(duration: number, sampleTime: number): number[] {
  const times: number[] = [0];
  for (let time = sampleTime; time < duration - 1e-12; time += sampleTime) {
    times.push(time);
  }
  times.push(duration);
  return times;
}

function interpolatePoint(points: TrajectoryPoint[], time: number): TrajectoryPoint {
  if (time <= points[0]!.time) {
    return {
      ...points[0]!,
      joints: [...points[0]!.joints],
      jointVelocity: [...points[0]!.jointVelocity],
      jointAcceleration: [...points[0]!.jointAcceleration]
    };
  }
  const last = points.at(-1)!;
  if (time >= last.time) {
    return {
      ...last,
      joints: [...last.joints],
      jointVelocity: [...last.jointVelocity],
      jointAcceleration: [...last.jointAcceleration]
    };
  }

  const nextIndex = points.findIndex((point) => point.time >= time);
  const next = points[nextIndex]!;
  const prev = points[nextIndex - 1]!;
  const ratio = (time - prev.time) / (next.time - prev.time);
  return {
    ...next,
    time,
    s: lerp(prev.s, next.s, ratio),
    sd: lerp(prev.sd, next.sd, ratio),
    sdd: lerp(prev.sdd, next.sdd, ratio),
    joints: lerpArray(prev.joints, next.joints, ratio),
    jointVelocity: lerpArray(prev.jointVelocity, next.jointVelocity, ratio),
    jointAcceleration: lerpArray(prev.jointAcceleration, next.jointAcceleration, ratio),
    flange: lerpPose(prev.flange, next.flange, ratio),
    tcp: lerpPose(prev.tcp, next.tcp, ratio),
    diagnostics: []
  };
}

function lerp(a: number, b: number, ratio: number): number {
  return a + (b - a) * ratio;
}

function lerpArray(a: number[], b: number[], ratio: number): number[] {
  const length = Math.max(a.length, b.length);
  return Array.from({ length }, (_, index) => lerp(a[index] ?? 0, b[index] ?? 0, ratio));
}

function lerpPose(a: Pose, b: Pose, ratio: number): Pose {
  return {
    position: [
      lerp(a.position[0], b.position[0], ratio),
      lerp(a.position[1], b.position[1], ratio),
      lerp(a.position[2], b.position[2], ratio)
    ],
    quaternion: ratio < 0.5 ? a.quaternion : b.quaternion
  };
}
