import { rpyToQuaternion } from "../../math/poseMath.js";
import type { JointTarget, OffsetSpec, Pose, PoseTarget, SpeedSpec, ZoneSpec } from "../../kdl/types.js";
import { KdlStructuredError } from "../../kdl/rpc.js";
import type {
  GrlCallExpression,
  GrlDataDeclaration,
  GrlExpression,
  GrlObjectExpression,
  GrlOffsetExpression,
  GrlTargetDeclaration
} from "../ast/index.js";
import {
  type ConstantEvaluationContext,
  type ConstantValue,
  evaluateConstantExpression,
  evaluateNumberExpression
} from "./constantExpression.js";

export type CompiledGrlDataValue =
  | Pose
  | JointTarget
  | PoseTarget
  | SpeedSpec
  | ZoneSpec
  | OffsetSpec
  | Record<string, unknown>
  | string
  | number
  | boolean
  | number[];

export interface CompiledGrlDataDeclaration {
  name: string;
  storage: GrlDataDeclaration["storage"];
  typeName: string;
  value: CompiledGrlDataValue;
}

export interface CompiledGrlTargetDeclaration {
  name: string;
  target: JointTarget | PoseTarget;
}

export function compileGrlDataDeclaration(
  declaration: GrlDataDeclaration,
  context: ConstantEvaluationContext | number = {}
): CompiledGrlDataDeclaration {
  const evaluationContext = typeof context === "number" ? {} : context;
  return {
    name: declaration.name,
    storage: declaration.storage,
    typeName: declaration.typeName,
    value: compileByType(declaration.typeName, declaration.initializer, evaluationContext)
  };
}

export function compileGrlConstantValue(expression: GrlExpression): ConstantValue {
  return evaluateConstantExpression(expression);
}

export function compileGrlTargetDeclaration(
  declaration: GrlTargetDeclaration,
  context: ConstantEvaluationContext = {}
): CompiledGrlTargetDeclaration {
  const target = compileTargetExpression(declaration.target, context);
  return {
    name: declaration.name,
    target: {
      ...target,
      id: target.id ?? declaration.name
    }
  };
}

export function compileTargetExpression(
  expression: GrlExpression,
  context: ConstantEvaluationContext = {}
): JointTarget | PoseTarget {
  if (isObjectExpression(expression, "joint_target")) {
    return {
      joints: compileNumberArray(requiredProperty(expression, "joints"), {
        ...context,
        expectedKind: "angle",
        defaultUnit: "deg"
      })
    };
  }

  if (isObjectExpression(expression, "pose_target")) {
    const pose = compilePoseExpression(requiredProperty(expression, "pose"), context);
    const configExpression = findProperty(expression, "config");
    const config = configExpression ? compileRobotConfig(configExpression, context) : undefined;
    return {
      pose,
      ...(config ? { config } : {}),
      ...(findIdentifierName(expression, "tool") ? { tool: { id: findIdentifierName(expression, "tool") } as unknown as Pose } : {}),
      ...(findIdentifierName(expression, "frame") ? { frame: { id: findIdentifierName(expression, "frame") } as unknown as Pose } : {})
    };
  }

  throw compileError("GRL_UNSUPPORTED_TARGET", "Expected joint_target or pose_target expression");
}

export function compileSpeedExpression(
  expression: GrlExpression,
  context: ConstantEvaluationContext = {}
): SpeedSpec {
  if (!isCallExpression(expression)) {
    throw compileError("GRL_INVALID_SPEED", "Speed expression must be a call");
  }

  const first = expression.args[0];
  if (!first) {
    throw compileError("GRL_INVALID_SPEED", "Speed expression requires a numeric value");
  }

  if (expression.callee === "joint") {
    const value = evaluateConstantExpression(first, { ...context, defaultUnit: "%" });
    if (value.kind === "percent") {
      return { kind: "joint_percent", value: value.value as number };
    }
    return { kind: "joint_abs", velocity: evaluateNumberExpression(first, { ...context, expectedKind: "angular_velocity", defaultUnit: "deg/s" }) };
  }

  if (expression.callee === "linear") {
    return {
      kind: "linear",
      velocity: evaluateNumberExpression(first, { ...context, expectedKind: "linear_velocity", defaultUnit: "mm/s" }),
      ...compileAcceleration(expression, context)
    };
  }

  if (expression.callee === "angular") {
    return {
      kind: "linear",
      velocity: 0,
      angularVelocity: evaluateNumberExpression(first, { ...context, expectedKind: "angular_velocity", defaultUnit: "deg/s" }),
      ...compileAcceleration(expression, context)
    };
  }

  throw compileError("GRL_INVALID_SPEED", `Unsupported speed expression: ${expression.callee}`);
}

export function compileZoneExpression(
  expression: GrlExpression,
  context: ConstantEvaluationContext = {}
): ZoneSpec {
  if (expression.kind === "IdentifierExpression") {
    if (expression.name === "fine") {
      return { kind: "fine" };
    }
    if (expression.name === "continuous") {
      return { kind: "continuous" };
    }
  }

  if (isCallExpression(expression) && expression.callee === "z") {
    const first = expression.args[0];
    if (!first) {
      throw compileError("GRL_INVALID_ZONE", "z(...) requires a distance");
    }
    return { kind: "distance", value: evaluateNumberExpression(first, { ...context, expectedKind: "length", defaultUnit: "mm" }) };
  }

  if (isCallExpression(expression) && expression.callee === "cnt") {
    const first = expression.args[0];
    if (!first) {
      throw compileError("GRL_INVALID_ZONE", "cnt(...) requires a percent value");
    }
    return { kind: "cnt", value: evaluateNumberExpression(first, context) };
  }

  throw compileError("GRL_INVALID_ZONE", "Unsupported zone expression");
}

export function compileOffsetExpression(
  expression: GrlOffsetExpression,
  context: ConstantEvaluationContext = {}
): OffsetSpec {
  const xyz: [number, number, number] = [0, 0, 0];
  for (const axis of expression.axes) {
    const index = axis.axis === "x" ? 0 : axis.axis === "y" ? 1 : 2;
    xyz[index] = evaluateNumberExpression(axis.value, { ...context, expectedKind: "length", defaultUnit: "mm" });
  }

  return {
    mode: expression.mode,
    ...(expression.frameName ? { frameId: expression.frameName } : {}),
    xyz
  };
}

function compileByType(
  typeName: string,
  expression: GrlExpression,
  context: ConstantEvaluationContext = {}
): CompiledGrlDataValue {
  if (typeName === "speed") {
    return compileSpeedExpression(expression, context);
  }
  if (typeName === "zone") {
    return compileZoneExpression(expression, context);
  }
  if (typeName === "pose") {
    return compilePoseExpression(expression, context);
  }
  if (typeName === "pose_target" || typeName === "joint_target") {
    return compileTargetExpression(expression, context);
  }
  if (expression.kind === "OffsetExpression") {
    return compileOffsetExpression(expression, context);
  }
  if (typeName === "tool" && isObjectExpression(expression, "tool")) {
    return {
      tcp: compilePoseExpression(requiredProperty(expression, "tcp"), context),
      ...(findProperty(expression, "mass") ? { mass: evaluateNumberExpression(findProperty(expression, "mass")!, { ...context, expectedKind: "mass", defaultUnit: "kg" }) } : {}),
      ...(findProperty(expression, "cog") ? { cog: compileNumberArray(findProperty(expression, "cog")!, { ...context, expectedKind: "length", defaultUnit: "mm" }) } : {})
    };
  }
  if (typeName === "frame" && isObjectExpression(expression, "frame")) {
    return {
      origin: compilePoseExpression(requiredProperty(expression, "origin"), context)
    };
  }
  if (isConstantScalarExpression(expression)) return evaluateConstantExpression(expression, context).value as number | boolean | string;
  if (expression.kind === "StringLiteral" || expression.kind === "BooleanLiteral") {
    return expression.value;
  }
  if (expression.kind === "ArrayExpression") {
    return compileNumberArray(expression);
  }

  return { kind: expression.kind };
}

function compilePoseExpression(
  expression: GrlExpression,
  context: ConstantEvaluationContext = {}
): Pose {
  if (!isCallExpression(expression) || (expression.callee !== "pose" && expression.callee !== "poseq")) {
    throw compileError("GRL_INVALID_POSE", "Expected pose(...) or poseq(...) expression");
  }

  if (expression.callee === "pose") {
    if (expression.args.length !== 6) {
      throw compileError("GRL_INVALID_POSE", "pose(...) requires 6 arguments");
    }
    const values = expression.args.map((arg, index) =>
      evaluateNumberExpression(arg, {
        ...context,
        expectedKind: index < 3 ? "length" : "angle",
        defaultUnit: index < 3 ? "mm" : "deg"
      })
    );
    return {
      position: [values[0]!, values[1]!, values[2]!],
      quaternion: rpyToQuaternion([values[3]!, values[4]!, values[5]!])
    };
  }

  if (expression.args.length !== 7) {
    throw compileError("GRL_INVALID_POSE", "poseq(...) requires 7 arguments");
  }
  const values = expression.args.map((arg, index) =>
    index < 3
      ? evaluateNumberExpression(arg, { ...context, expectedKind: "length", defaultUnit: "mm" })
      : evaluateNumberExpression(arg, context)
  );
  return {
    position: [values[0]!, values[1]!, values[2]!],
    quaternion: [values[3]!, values[4]!, values[5]!, values[6]!]
  };
}

function compileRobotConfig(expression: GrlExpression, context: ConstantEvaluationContext = {}) {
  if (!isCallExpression(expression) || expression.callee !== "robot_config") {
    throw compileError("GRL_INVALID_CONFIG", "Expected robot_config(...)");
  }
  const values = expression.args.map((arg) => {
    return evaluateNumberExpression(arg, context) as -1 | 0 | 1;
  });
  return {
    ...(values[0] !== undefined ? { shoulder: values[0] } : {}),
    ...(values[1] !== undefined ? { elbow: values[1] } : {}),
    ...(values[2] !== undefined ? { wrist: values[2] } : {})
  };
}

function compileNumberArray(
  expression: GrlExpression,
  context: ConstantEvaluationContext = {}
): number[] {
  if (expression.kind !== "ArrayExpression") {
    throw compileError("GRL_INVALID_ARRAY", "Expected numeric array");
  }
  return expression.elements.map((element) => evaluateNumberExpression(element, context));
}

function compileAcceleration(
  expression: GrlCallExpression,
  context: ConstantEvaluationContext = {}
): { acceleration?: number } {
  for (let index = 1; index < expression.args.length; index += 1) {
    const marker = expression.args[index];
    const value = expression.args[index + 1];
    if (marker?.kind === "IdentifierExpression" && marker.name === "acc" && value) {
      return { acceleration: evaluateNumberExpression(value, { ...context, expectedKind: "linear_acceleration", defaultUnit: "mm/s2" }) };
    }
  }
  return {};
}

function requiredProperty(expression: GrlObjectExpression, key: string): GrlExpression {
  const property = findProperty(expression, key);
  if (!property) {
    throw compileError("GRL_MISSING_PROPERTY", `${expression.typeName} is missing ${key}`);
  }
  return property;
}

function findProperty(expression: GrlObjectExpression, key: string): GrlExpression | undefined {
  return expression.properties.find((property) => property.key === key)?.value;
}

function findIdentifierName(expression: GrlObjectExpression, key: string): string | undefined {
  const value = findProperty(expression, key);
  return value?.kind === "IdentifierExpression" ? value.name : undefined;
}

function isObjectExpression(expression: GrlExpression, typeName: string): expression is GrlObjectExpression {
  return expression.kind === "ObjectExpression" && expression.typeName === typeName;
}

function isCallExpression(expression: GrlExpression): expression is GrlCallExpression {
  return expression.kind === "CallExpression";
}

function isConstantScalarExpression(expression: GrlExpression): boolean {
  return [
    "NumberLiteral",
    "StringLiteral",
    "BooleanLiteral",
    "UnaryExpression",
    "BinaryExpression",
    "CallExpression",
    "IdentifierExpression",
    "UnitExpression"
  ].includes(expression.kind);
}

function compileError(code: string, message: string): KdlStructuredError {
  return new KdlStructuredError(code, message);
}
