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 {
  GrlArrayExpression,
  GrlCallExpression,
  GrlDataDeclaration,
  GrlExpression,
  GrlNumberLiteral,
  GrlObjectExpression,
  GrlOffsetExpression,
  GrlTargetDeclaration
} from "../ast/index.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): CompiledGrlDataDeclaration {
  return {
    name: declaration.name,
    storage: declaration.storage,
    typeName: declaration.typeName,
    value: compileByType(declaration.typeName, declaration.initializer)
  };
}

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

export function compileTargetExpression(expression: GrlExpression): JointTarget | PoseTarget {
  if (isObjectExpression(expression, "joint_target")) {
    return {
      joints: compileNumberArray(requiredProperty(expression, "joints"))
    };
  }

  if (isObjectExpression(expression, "pose_target")) {
    const pose = compilePoseExpression(requiredProperty(expression, "pose"));
    const configExpression = findProperty(expression, "config");
    const config = configExpression ? compileRobotConfig(configExpression) : 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): SpeedSpec {
  if (!isCallExpression(expression)) {
    throw compileError("GRL_INVALID_SPEED", "Speed expression must be a call");
  }

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

  if (expression.callee === "joint") {
    if (first.unit?.kind === "percent") {
      return { kind: "joint_percent", value: first.unit.normalizedValue };
    }
    return { kind: "joint_abs", velocity: normalizedNumber(first) };
  }

  if (expression.callee === "linear") {
    return { kind: "linear", velocity: normalizedNumber(first), ...compileAcceleration(expression) };
  }

  if (expression.callee === "angular") {
    return { kind: "linear", velocity: 0, angularVelocity: normalizedNumber(first), ...compileAcceleration(expression) };
  }

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

export function compileZoneExpression(expression: GrlExpression): 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 || first.kind !== "NumberLiteral") {
      throw compileError("GRL_INVALID_ZONE", "z(...) requires a distance");
    }
    return { kind: "distance", value: normalizedNumber(first) };
  }

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

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

export function compileOffsetExpression(expression: GrlOffsetExpression): 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] = normalizedNumber(axis.value);
  }

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

function compileByType(typeName: string, expression: GrlExpression): CompiledGrlDataValue {
  if (typeName === "speed") {
    return compileSpeedExpression(expression);
  }
  if (typeName === "zone") {
    return compileZoneExpression(expression);
  }
  if (typeName === "pose") {
    return compilePoseExpression(expression);
  }
  if (typeName === "pose_target" || typeName === "joint_target") {
    return compileTargetExpression(expression);
  }
  if (expression.kind === "OffsetExpression") {
    return compileOffsetExpression(expression);
  }
  if (typeName === "tool" && isObjectExpression(expression, "tool")) {
    return {
      tcp: compilePoseExpression(requiredProperty(expression, "tcp")),
      ...(findProperty(expression, "mass") ? { mass: normalizedNumber(findProperty(expression, "mass") as GrlNumberLiteral) } : {}),
      ...(findProperty(expression, "cog") ? { cog: compileNumberArray(findProperty(expression, "cog")!) } : {})
    };
  }
  if (typeName === "frame" && isObjectExpression(expression, "frame")) {
    return {
      origin: compilePoseExpression(requiredProperty(expression, "origin"))
    };
  }
  if (expression.kind === "NumberLiteral") {
    return normalizedNumber(expression);
  }
  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): Pose {
  if (!isCallExpression(expression) || (expression.callee !== "pose" && expression.callee !== "poseq")) {
    throw compileError("GRL_INVALID_POSE", "Expected pose(...) or poseq(...) expression");
  }

  const values = expression.args.map((arg) => {
    if (arg.kind !== "NumberLiteral") {
      throw compileError("GRL_INVALID_POSE", "Pose arguments must be numeric");
    }
    return normalizedNumber(arg);
  });

  if (expression.callee === "pose") {
    if (values.length !== 6) {
      throw compileError("GRL_INVALID_POSE", "pose(...) requires 6 arguments");
    }
    return {
      position: [values[0]!, values[1]!, values[2]!],
      quaternion: rpyToQuaternion([values[3]!, values[4]!, values[5]!])
    };
  }

  if (values.length !== 7) {
    throw compileError("GRL_INVALID_POSE", "poseq(...) requires 7 arguments");
  }
  return {
    position: [values[0]!, values[1]!, values[2]!],
    quaternion: [values[3]!, values[4]!, values[5]!, values[6]!]
  };
}

function compileRobotConfig(expression: GrlExpression) {
  if (!isCallExpression(expression) || expression.callee !== "robot_config") {
    throw compileError("GRL_INVALID_CONFIG", "Expected robot_config(...)");
  }
  const values = expression.args.map((arg) => {
    if (arg.kind !== "NumberLiteral") {
      throw compileError("GRL_INVALID_CONFIG", "robot_config arguments must be numeric");
    }
    return arg.value 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): number[] {
  if (expression.kind !== "ArrayExpression") {
    throw compileError("GRL_INVALID_ARRAY", "Expected numeric array");
  }
  return expression.elements.map((element) => {
    if (element.kind !== "NumberLiteral") {
      throw compileError("GRL_INVALID_ARRAY", "Array elements must be numeric");
    }
    return normalizedNumber(element);
  });
}

function compileAcceleration(expression: GrlCallExpression): { 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?.kind === "NumberLiteral") {
      return { acceleration: normalizedNumber(value) };
    }
  }
  return {};
}

function normalizedNumber(expression: GrlNumberLiteral): number {
  return expression.unit?.normalizedValue ?? expression.value;
}

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 compileError(code: string, message: string): KdlStructuredError {
  return new KdlStructuredError(code, message);
}
