import { KdlStructuredError } from "./rpc.js";
import type { MotionDiagnostic, TrapProfileOptions, TrapProfileResult, TrapSample } from "./types.js";

export function makeTrapProfile(length: number, options: TrapProfileOptions): TrapProfileResult {
  validateTrapInputs(length, options);

  if (length === 0) {
    return {
      ok: true,
      type: "triangle",
      length,
      duration: 0,
      tAccel: 0,
      tConst: 0,
      tDecel: 0,
      vPeak: 0,
      samples: [
        {
          index: 0,
          time: 0,
          s: 0,
          sd: 0,
          sdd: 0
        }
      ],
      diagnostics: [
        {
          severity: "info",
          code: "KDL_TRAP_ZERO_LENGTH",
          message: "Trap profile length is zero"
        }
      ]
    };
  }

  const startVelocity = options.startVelocity ?? 0;
  const endVelocity = options.endVelocity ?? 0;
  const maxVelocity = options.maxVelocity;
  const maxAcceleration = options.maxAcceleration;
  const dAccelToMax = distanceForVelocityChange(startVelocity, maxVelocity, maxAcceleration);
  const dDecelFromMax = distanceForVelocityChange(endVelocity, maxVelocity, maxAcceleration);
  const diagnostics: MotionDiagnostic[] = [];
  let type: TrapProfileResult["type"] = "trapezoid";
  let vPeak = maxVelocity;
  let tConst = 0;

  if (dAccelToMax + dDecelFromMax <= length) {
    tConst = (length - dAccelToMax - dDecelFromMax) / maxVelocity;
  } else {
    type = "triangle";
    vPeak = Math.sqrt(Math.max(0, maxAcceleration * length + (startVelocity ** 2 + endVelocity ** 2) / 2));
    if (vPeak + 1e-12 < Math.max(startVelocity, endVelocity)) {
      throw new KdlStructuredError(
        "KDL_INVALID_TRAP_PROFILE",
        "Profile length is too short for the requested startVelocity/endVelocity"
      );
    }
    tConst = 0;
    diagnostics.push({
      severity: "info",
      code: "KDL_TRAP_TRIANGLE_PROFILE",
      message: "Trap profile length is too short to reach maxVelocity; using triangle profile"
    });
  }

  const tAccel = Math.max(0, (vPeak - startVelocity) / maxAcceleration);
  const tDecel = Math.max(0, (vPeak - endVelocity) / maxAcceleration);
  const duration = tAccel + tConst + tDecel;
  const samples = sampleProfile({
    length,
    sampleTime: options.sampleTime,
    startVelocity,
    endVelocity,
    maxAcceleration,
    tAccel,
    tConst,
    tDecel,
    vPeak,
    duration
  });

  return {
    ok: true,
    type,
    length,
    duration,
    tAccel,
    tConst,
    tDecel,
    vPeak,
    samples,
    diagnostics
  };
}

export function sampleTrapProfile(length: number, options: TrapProfileOptions): TrapSample[] {
  return makeTrapProfile(length, options).samples;
}

interface ProfileSegments {
  length: number;
  sampleTime: number;
  startVelocity: number;
  endVelocity: number;
  maxAcceleration: number;
  tAccel: number;
  tConst: number;
  tDecel: number;
  vPeak: number;
  duration: number;
}

function validateTrapInputs(length: number, options: TrapProfileOptions): void {
  if (!Number.isFinite(length) || length < 0) {
    throw new KdlStructuredError("KDL_INVALID_TRAP_PROFILE", "Trap profile length must be a finite non-negative number");
  }
  if (!Number.isFinite(options.maxVelocity) || options.maxVelocity <= 0) {
    throw new KdlStructuredError("KDL_INVALID_TRAP_PROFILE", "maxVelocity must be a finite positive number");
  }
  if (!Number.isFinite(options.maxAcceleration) || options.maxAcceleration <= 0) {
    throw new KdlStructuredError("KDL_INVALID_TRAP_PROFILE", "maxAcceleration must be a finite positive number");
  }
  if (!Number.isFinite(options.sampleTime) || options.sampleTime <= 0) {
    throw new KdlStructuredError("KDL_INVALID_TRAP_PROFILE", "sampleTime must be a finite positive number");
  }

  const startVelocity = options.startVelocity ?? 0;
  const endVelocity = options.endVelocity ?? 0;
  if (length === 0 && (startVelocity > 0 || endVelocity > 0)) {
    throw new KdlStructuredError(
      "KDL_INVALID_TRAP_PROFILE",
      "Zero-length trap profile requires zero startVelocity and endVelocity"
    );
  }
  if (!Number.isFinite(startVelocity) || startVelocity < 0 || startVelocity > options.maxVelocity) {
    throw new KdlStructuredError(
      "KDL_INVALID_TRAP_PROFILE",
      "startVelocity must be finite, non-negative, and no greater than maxVelocity"
    );
  }
  if (!Number.isFinite(endVelocity) || endVelocity < 0 || endVelocity > options.maxVelocity) {
    throw new KdlStructuredError(
      "KDL_INVALID_TRAP_PROFILE",
      "endVelocity must be finite, non-negative, and no greater than maxVelocity"
    );
  }
}

function distanceForVelocityChange(fromVelocity: number, toVelocity: number, acceleration: number): number {
  return Math.max(0, (toVelocity ** 2 - fromVelocity ** 2) / (2 * acceleration));
}

function sampleProfile(profile: ProfileSegments): TrapSample[] {
  if (profile.duration === 0) {
    return [
      {
        index: 0,
        time: 0,
        s: 0,
        sd: 0,
        sdd: 0
      }
    ];
  }

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

  return times.map((time, index) => {
    const sample = sampleAtTime(profile, time);
    return {
      index,
      time,
      s: index === 0 ? 0 : index === times.length - 1 ? 1 : clamp01(sample.distance / profile.length),
      sd: sample.velocity / profile.length,
      sdd: sample.acceleration / profile.length
    };
  });
}

function sampleAtTime(profile: ProfileSegments, time: number): {
  distance: number;
  velocity: number;
  acceleration: number;
} {
  const accelDistance =
    profile.startVelocity * profile.tAccel + 0.5 * profile.maxAcceleration * profile.tAccel ** 2;
  const constDistance = profile.vPeak * profile.tConst;
  const accelEnd = profile.tAccel;
  const constEnd = profile.tAccel + profile.tConst;

  if (time <= accelEnd) {
    return {
      distance: profile.startVelocity * time + 0.5 * profile.maxAcceleration * time ** 2,
      velocity: profile.startVelocity + profile.maxAcceleration * time,
      acceleration: profile.maxAcceleration
    };
  }

  if (time <= constEnd) {
    const localTime = time - profile.tAccel;
    return {
      distance: accelDistance + profile.vPeak * localTime,
      velocity: profile.vPeak,
      acceleration: 0
    };
  }

  const localTime = Math.min(time - constEnd, profile.tDecel);
  return {
    distance: accelDistance + constDistance + profile.vPeak * localTime - 0.5 * profile.maxAcceleration * localTime ** 2,
    velocity: Math.max(profile.endVelocity, profile.vPeak - profile.maxAcceleration * localTime),
    acceleration: -profile.maxAcceleration
  };
}

function clamp01(value: number): number {
  return Math.min(1, Math.max(0, value));
}
