import type {
  JsonObject,
  OlpCalibrationRecord,
  OlpCollisionObject,
  OlpPath,
  OlpPathPoint,
  OlpProcessTemplate,
  OlpProjectModel,
  OlpTarget,
  Pose6D,
  Vec3
} from "./model.js";

export type GeometrySource =
  | { kind: "points"; points: Vec3[] }
  | { kind: "edge"; start: Vec3; end: Vec3; samples: number }
  | { kind: "polyline"; points: Vec3[]; spacing?: number }
  | { kind: "curve"; controlPoints: Vec3[]; samples: number };

export interface GeometryPathOptions {
  pathId: string;
  targetPrefix?: string;
  motion?: "movej" | "movel";
  speedId?: string;
  zoneId?: string;
  toolId?: string;
  frameId?: string;
  zOffset?: number;
}

export interface GeometryPathResult {
  targets: OlpTarget[];
  path: OlpPath;
}

export interface CollisionSample {
  time: number;
  pointId: string;
  position: Vec3;
}

export interface CollisionHit {
  objectId: string;
  pointId: string;
  time: number;
  distance: number;
}

export interface CollisionReport {
  ok: boolean;
  hits: CollisionHit[];
}

export type CollisionCheckResult = CollisionReport;

export interface ControllerCheckSample {
  pointId: string;
  offline: Pose6D;
  measured: Pose6D;
  speedPercent: number;
}

export interface ControllerCheckReport {
  status: "pass" | "warn" | "fail";
  speedPercent: number;
  maxPositionErrorMm: number;
  maxOrientationErrorDeg: number;
  samples: Array<ControllerCheckSample & {
    positionErrorMm: number;
    orientationErrorDeg: number;
  }>;
  boundaries: string[];
}

export interface CommercialExtensionBoundary {
  feature: string;
  mvp: boolean;
  boundary: string;
}

export function generatePathFromGeometry(source: GeometrySource, options: GeometryPathOptions): GeometryPathResult {
  const targetPrefix = options.targetPrefix ?? options.pathId;
  const points = sampleGeometry(source).map((point): Vec3 => [
    point[0],
    point[1],
    point[2] + (options.zOffset ?? 0)
  ]);
  const targets = points.map((point, index): OlpTarget => {
    const name = `${targetPrefix}_${String(index).padStart(2, "0")}`;
    return {
      id: `target_${name}`,
      name,
      kind: "pose",
      pose: [point[0], point[1], point[2], 0, 0, 0],
      ...(options.toolId ? { toolId: options.toolId } : {}),
      ...(options.frameId ? { frameId: options.frameId } : {})
    };
  });
  const pathPoints = targets.map((target, index): OlpPathPoint => ({
    id: `${options.pathId}_p${String(index).padStart(2, "0")}`,
    name: `p${String(index).padStart(2, "0")}`,
    motion: index === 0 ? (options.motion ?? "movej") : "movel",
    targetId: target.id,
    ...(options.speedId ? { speedId: options.speedId } : {}),
    ...(options.zoneId ? { zoneId: options.zoneId } : {}),
    ...(options.toolId ? { toolId: options.toolId } : {}),
    ...(options.frameId ? { frameId: options.frameId } : {})
  }));
  return {
    targets,
    path: {
      id: options.pathId,
      name: options.pathId,
      source: {
        type: source.kind,
        samples: points.length
      },
      defaults: {
        ...(options.speedId ? { speedId: options.speedId } : {}),
        ...(options.zoneId ? { zoneId: options.zoneId } : {}),
        ...(options.toolId ? { toolId: options.toolId } : {}),
        ...(options.frameId ? { frameId: options.frameId } : {})
      },
      points: pathPoints
    }
  };
}

export function detectBasicCollisions(samples: CollisionSample[], objects: OlpCollisionObject[], clearanceMm = 0): CollisionReport {
  const hits: CollisionHit[] = [];
  for (const sample of samples) {
    for (const object of objects) {
      const distance = signedDistanceToObject(sample.position, object);
      if (distance <= clearanceMm) {
        hits.push({
          objectId: object.id,
          pointId: sample.pointId,
          time: sample.time,
          distance
        });
      }
    }
  }
  return {
    ok: hits.length === 0,
    hits
  };
}

export const checkBasicCollisions = detectBasicCollisions;

export function locateCollisionTime(report: CollisionReport): number | undefined {
  return report.hits.reduce<number | undefined>((earliest, hit) => (
    earliest === undefined || hit.time < earliest ? hit.time : earliest
  ), undefined);
}

export function applyCalibrationRecords(project: OlpProjectModel, records: OlpCalibrationRecord[]): OlpProjectModel {
  const next = JSON.parse(JSON.stringify(project)) as OlpProjectModel;
  for (const record of records) {
    if (record.kind === "tcp" && record.poseDelta) {
      const tool = next.resources.tools.find((item) => item.id === record.targetResourceId);
      if (tool) tool.tcp = addPose(tool.tcp, record.poseDelta);
    }
    if ((record.kind === "frame" || record.kind === "base") && record.poseDelta) {
      const frame = next.resources.frames.find((item) => item.id === record.targetResourceId);
      if (frame) frame.pose = addPose(frame.pose, record.poseDelta);
    }
    if (record.kind === "external_axis" && record.axisOffset !== undefined) {
      const axis = next.externalAxes.find((item) => item.id === record.targetResourceId);
      if (axis) {
        axis.metadata = {
          ...(axis.metadata ?? {}),
          calibrationOffset: record.axisOffset
        } as JsonObject;
      }
    }
    next.calibrations = upsertById(next.calibrations, record);
  }
  return next;
}

export function createResourceLibraryTemplate(project: OlpProjectModel): JsonObject {
  return {
    robots: project.resources.robots.map((robot) => ({ id: robot.id, brand: robot.brand, model: robot.model })),
    tools: project.resources.tools.map((tool) => ({ id: tool.id, tcp: tool.tcp })),
    frames: project.resources.frames.map((frame) => ({ id: frame.id, pose: frame.pose })),
    collisionObjects: project.resources.collisionObjects.map((object) => ({ id: object.id, shape: object.shape }))
  };
}

export function instantiateProcessTemplate(template: OlpProcessTemplate, operationId: string): JsonObject {
  return {
    operationId,
    operationKind: template.operationKind,
    defaults: template.defaults,
    deliveryTags: template.deliveryTags
  };
}

export function compareControllerLowSpeedRun(
  samples: ControllerCheckSample[],
  tolerances: { positionMm: number; orientationDeg: number },
  boundaries = controllerVerificationBoundaries()
): ControllerCheckReport {
  const evaluated = samples.map((sample) => {
    const positionErrorMm = distance3(sample.offline, sample.measured);
    const orientationErrorDeg = Math.max(
      Math.abs(sample.offline[3] - sample.measured[3]),
      Math.abs(sample.offline[4] - sample.measured[4]),
      Math.abs(sample.offline[5] - sample.measured[5])
    );
    return {
      ...sample,
      positionErrorMm,
      orientationErrorDeg
    };
  });
  const maxPositionErrorMm = Math.max(0, ...evaluated.map((sample) => sample.positionErrorMm));
  const maxOrientationErrorDeg = Math.max(0, ...evaluated.map((sample) => sample.orientationErrorDeg));
  const status = maxPositionErrorMm > tolerances.positionMm || maxOrientationErrorDeg > tolerances.orientationDeg
    ? "fail"
    : samples.some((sample) => sample.speedPercent > 25)
      ? "warn"
      : "pass";
  return {
    status,
    speedPercent: Math.max(0, ...samples.map((sample) => sample.speedPercent)),
    maxPositionErrorMm,
    maxOrientationErrorDeg,
    samples: evaluated,
    boundaries
  };
}

export function controllerVerificationBoundaries(): string[] {
  return [
    "MVP records low-speed sampled offline/field deltas; it does not open a live controller communication session.",
    "MVP compares taught point poses and max error; it does not guarantee high-precision dynamic trajectory reproduction.",
    "MVP uses sampled geometry and primitive collision volumes; it does not include a full CAD kernel."
  ];
}

export function commercialExtensionBoundaries(): CommercialExtensionBoundary[] {
  return [
    {
      feature: "resource_library",
      mvp: true,
      boundary: "JSON serializable robot/tool/frame/collision object catalog only."
    },
    {
      feature: "process_templates",
      mvp: true,
      boundary: "Template defaults and delivery tags only; no proprietary process parameter solver."
    },
    {
      feature: "delivery_package",
      mvp: true,
      boundary: "Stable JSON/HTML/text package prototype; compression and signing can be added later."
    },
    {
      feature: "cad_kernel",
      mvp: false,
      boundary: "Exact CAD B-rep import and boolean collision are outside this MVP."
    },
    {
      feature: "real_controller_communication",
      mvp: false,
      boundary: "Live controller upload/download and safety interlocks are outside this MVP."
    }
  ];
}

function sampleGeometry(source: GeometrySource): Vec3[] {
  if (source.kind === "points") {
    return source.points;
  }
  if (source.kind === "edge") {
    return interpolate(source.start, source.end, Math.max(2, source.samples));
  }
  if (source.kind === "polyline") {
    if (!source.spacing) return source.points;
    const result: Vec3[] = [];
    for (let index = 0; index < source.points.length - 1; index += 1) {
      const start = source.points[index]!;
      const end = source.points[index + 1]!;
      const count = Math.max(2, Math.ceil(distanceVec3(start, end) / source.spacing) + 1);
      const segment = interpolate(start, end, count);
      result.push(...(index === 0 ? segment : segment.slice(1)));
    }
    return result;
  }
  const count = Math.max(2, source.samples);
  const result: Vec3[] = [];
  for (let index = 0; index < count; index += 1) {
    const t = count === 1 ? 0 : index / (count - 1);
    result.push(bezier(source.controlPoints, t));
  }
  return result;
}

function interpolate(start: Vec3, end: Vec3, count: number): Vec3[] {
  return Array.from({ length: count }, (_, index) => {
    const t = count === 1 ? 0 : index / (count - 1);
    return [
      start[0] + (end[0] - start[0]) * t,
      start[1] + (end[1] - start[1]) * t,
      start[2] + (end[2] - start[2]) * t
    ];
  });
}

function bezier(points: Vec3[], t: number): Vec3 {
  if (points.length === 0) return [0, 0, 0];
  let current = points.map((point) => [...point] as Vec3);
  while (current.length > 1) {
    current = current.slice(0, -1).map((point, index): Vec3 => [
      point[0] + (current[index + 1]![0] - point[0]) * t,
      point[1] + (current[index + 1]![1] - point[1]) * t,
      point[2] + (current[index + 1]![2] - point[2]) * t
    ]);
  }
  return current[0]!;
}

function signedDistanceToObject(point: Vec3, object: OlpCollisionObject): number {
  const center = object.pose.slice(0, 3) as Vec3;
  if (object.shape.kind === "sphere") {
    return distanceVec3(point, center) - object.shape.radius;
  }
  if (object.shape.kind === "capsule") {
    const half = object.shape.height / 2;
    const clampedZ = Math.max(center[2] - half, Math.min(center[2] + half, point[2]));
    return distanceVec3(point, [center[0], center[1], clampedZ]) - object.shape.radius;
  }
  const half = object.shape.size.map((value) => value / 2) as Vec3;
  const delta: Vec3 = [
    Math.max(Math.abs(point[0] - center[0]) - half[0], 0),
    Math.max(Math.abs(point[1] - center[1]) - half[1], 0),
    Math.max(Math.abs(point[2] - center[2]) - half[2], 0)
  ];
  const outsideDistance = distanceVec3(delta, [0, 0, 0]);
  const inside = Math.abs(point[0] - center[0]) <= half[0]
    && Math.abs(point[1] - center[1]) <= half[1]
    && Math.abs(point[2] - center[2]) <= half[2];
  return inside ? -Math.min(half[0], half[1], half[2]) : outsideDistance;
}

function addPose(left: Pose6D, right: Pose6D): Pose6D {
  return [
    left[0] + right[0],
    left[1] + right[1],
    left[2] + right[2],
    left[3] + right[3],
    left[4] + right[4],
    left[5] + right[5]
  ];
}

function upsertById<T extends { id: string }>(items: T[], item: T): T[] {
  const index = items.findIndex((entry) => entry.id === item.id);
  if (index === -1) return [...items, item];
  const next = [...items];
  next[index] = item;
  return next;
}

function distance3(left: Pose6D, right: Pose6D): number {
  return distanceVec3([left[0], left[1], left[2]], [right[0], right[1], right[2]]);
}

function distanceVec3(left: Vec3, right: Vec3): number {
  return Math.hypot(left[0] - right[0], left[1] - right[1], left[2] - right[2]);
}
