import { world, system, MolangVariableMap } from "@minecraft/server";
import { getBlockDistance } from "../utilities/helpers";

const BASE_MOVE_SPEED = 0.15; // Default horizontal movement speed
const VERTICAL_SPEED = 0.5; // Faster speed for Y-axis movement
const TOLERANCE = 1.1; // Threshold to consider the entity "arrived" at the target

// Function to calculate average speed based on the distance to the target
function calculateAverageSpeed(entity, distance) {
  const MIN_SPEED = 0.1; // Minimum speed limit to avoid overly slow movement
  const MAX_SPEED = 0.25; // Maximum speed limit for faster movement

  // Calculate a speed proportional to the distance, within limits
  let avgSpeed = BASE_MOVE_SPEED + distance / 10; // Adjust this formula for desired scaling
  avgSpeed = Math.max(MIN_SPEED, Math.min(avgSpeed, MAX_SPEED)); // Clamp speed between min and max
  if (entity.hasTag("hb_hr:ultimate_breeze_projectile")) {
    avgSpeed += 0.1;
    return avgSpeed;
  } else {
    return avgSpeed;
  }
}

// Function to calculate vector between two points and apply average speed
function calculateImpulse(currentPos, targetPos, avgSpeed) {
  const impulse = {
    x: targetPos.x - currentPos.x,
    y: targetPos.y - currentPos.y,
    z: targetPos.z - currentPos.z,
  };

  // Normalize the vector to apply consistent movement speed
  const magnitude = Math.sqrt(impulse.x * impulse.x + impulse.y * impulse.y + impulse.z * impulse.z);
  if (magnitude > 0) {
    impulse.x /= magnitude;
    impulse.y /= magnitude;
    impulse.z /= magnitude;
  }

  // Apply average speed
  impulse.x *= avgSpeed;

  // Adjust vertical speed based on the Y difference
  const verticalDistance = Math.abs(targetPos.y - currentPos.y);
  if (verticalDistance > 2) {
    impulse.y *= VERTICAL_SPEED;
  } else {
    impulse.y *= avgSpeed;
  }

  impulse.z *= avgSpeed;

  return impulse;
}

// Function to check if entity has reached the target position
function hasReachedTarget(currentPos, targetPos) {
  const distance = Math.sqrt(Math.pow(targetPos.x - currentPos.x, 2) + Math.pow(targetPos.y - currentPos.y, 2) + Math.pow(targetPos.z - currentPos.z, 2));
  return distance <= TOLERANCE;
}

// Function to move entity to each coordinate in a loop
export function moveEntityInLoop(entity, cursedCoreBlock) {
  if (!entity || !entity.isValid()) return;
  const dist = getBlockDistance(entity.location, cursedCoreBlock.location);
  // world.sendMessage(`Block distance: ${dist.toFixed(3)}`);
  if (dist.toFixed(3) > 9) {
    try {
      entity.dimension.spawnParticle("hb_hr:lost_soul_state_change", entity.location);
    } catch (error) {}
    try {
      entity.dimension.spawnEntity("hb_hr:agressive_lost_soul", entity.location);
      entity.remove();
    } catch (error) {}
    return;
  }

  const coords1 = entity.getDynamicProperty("hb_hr_coords1");
  const coords2 = entity.getDynamicProperty("hb_hr_coords2");
  const coords3 = entity.getDynamicProperty("hb_hr_coords3");
  const coords4 = entity.getDynamicProperty("hb_hr_coords4");
  const coords5 = entity.getDynamicProperty("hb_hr_coords5");
  const coords6 = entity.getDynamicProperty("hb_hr_coords6");
  const coords7 = entity.getDynamicProperty("hb_hr_coords7");
  const coords8 = entity.getDynamicProperty("hb_hr_coords8");

  const coordsArray = [coords1, coords2, coords3, coords4, coords5, coords6, coords7, coords8];

  // Retrieve the current target index or set it to 0 if it doesn't exist yet
  let currentTargetIndex = entity.getDynamicProperty("hb_hr_currentTargetIndex");
  if (currentTargetIndex === undefined || currentTargetIndex === null) {
    currentTargetIndex = 0; // Initialize it on the first tick
  }

  function moveTowardsTarget(entity) {
    const currentPos = entity.location;
    let targetPos = coordsArray[currentTargetIndex];

    // 1) Ankommen prüfen und ggf. zum nächsten Wegpunkt springen
    if (hasReachedTarget(currentPos, targetPos)) {
      currentTargetIndex = (currentTargetIndex + 1) % coordsArray.length;
      entity.setDynamicProperty("hb_hr_currentTargetIndex", currentTargetIndex);
      targetPos = coordsArray[currentTargetIndex];
    }

    // 2) Deltas und Impuls berechnen
    const dx = targetPos.x - currentPos.x;
    const dy = targetPos.y - currentPos.y;
    const dz = targetPos.z - currentPos.z;

    const distance = Math.hypot(dx, dy, dz);
    const avgSpeed = calculateAverageSpeed(entity, distance);
    const impulse = calculateImpulse(currentPos, targetPos, avgSpeed);

    // 3) Rotation **jeden Tick** richtig setzen
    //    – yaw: horizontal, mit –dx für Bedrock-Konvention
    //    – pitch: vertikal, negativ = Blick nach oben
    const horiz = Math.hypot(dx, dz);
    const yaw = Math.atan2(-dx, dz) * (180 / Math.PI);
    const pitch = -Math.atan2(dy, horiz) * (180 / Math.PI);

    entity.setRotation({ x: pitch, y: yaw });

    // 4) Impuls anwenden
    entity.applyImpulse(impulse);
  }

  // function moveTowardsTarget(entity) {
  //   const currentPos = entity.location;
  //   const targetPos = coordsArray[currentTargetIndex];

  //   // Check if the entity has reached the current target
  //   if (hasReachedTarget(currentPos, targetPos)) {
  //     // Move to the next coordinate in the array
  //     currentTargetIndex = (currentTargetIndex + 1) % coordsArray.length;
  //     // Store the updated index back as a dynamic property
  //     entity.setDynamicProperty("hb_hr_currentTargetIndex", currentTargetIndex);
  //   }

  //   // Calculate the distance to the target
  //   const distanceToTarget = Math.sqrt(Math.pow(targetPos.x - currentPos.x, 2) + Math.pow(targetPos.y - currentPos.y, 2) + Math.pow(targetPos.z - currentPos.z, 2));

  //   // Calculate average speed based on distance
  //   const avgSpeed = calculateAverageSpeed(entity, distanceToTarget);

  //   // Calculate and apply impulse toward the current target with the average speed
  //   const impulse = calculateImpulse(currentPos, targetPos, avgSpeed);
  //   entity.applyImpulse(impulse);
  // }

  // Let this function be called every tick from outside
  moveTowardsTarget(entity);
}
