/*
 * Derived from Epic Fight (https://github.com/Epic-Fight/epicfight)
 * by the Epic Fight Team, licensed under GPLv3.
 * Modifications © 2026 TiedUp! Remake Contributors, distributed under GPLv3.
 */

package com.tiedup.remake.rig.math;

import java.util.ArrayList;
import java.util.Collection;
import java.util.HashSet;
import java.util.List;
import java.util.Set;
import java.util.stream.Collectors;
import java.util.stream.Stream;

import org.joml.Math;
import org.joml.Matrix3f;
import org.joml.Matrix4f;
import org.joml.Quaternionf;
import org.joml.Vector3f;

import com.mojang.blaze3d.vertex.PoseStack;

import net.minecraft.util.Mth;
import net.minecraft.world.entity.Entity;
import net.minecraft.world.level.ClipContext;
import net.minecraft.world.level.Level;
import net.minecraft.world.phys.HitResult;
import net.minecraft.world.phys.Vec3;

public class MathUtils {
	public static final Vec3 XP = new Vec3(1.0D, 0.0D, 0.0D);
	public static final Vec3 XN = new Vec3(-1.0D, 0.0D, 0.0D);
	public static final Vec3 YP = new Vec3(0.0D, 1.0D, 0.0D);
	public static final Vec3 YN = new Vec3(0.0D, -1.0D, 0.0D);
	public static final Vec3 ZP = new Vec3(0.0D, 0.0D, 1.0D);
	public static final Vec3 ZN = new Vec3(0.0D, 0.0D, -1.0D);
	
	public static OpenMatrix4f getModelMatrixIntegral(float xPosO, float xPos, float yPosO, float yPos, float zPosO, float zPos, float xRotO, float xRot, float yRotO, float yRot, float partialTick, float scaleX, float scaleY, float scaleZ) {
		OpenMatrix4f modelMatrix = new OpenMatrix4f();
		Vec3f translation = new Vec3f(-(xPosO + (xPos - xPosO) * partialTick), ((yPosO + (yPos - yPosO) * partialTick)), -(zPosO + (zPos - zPosO) * partialTick));
		float partialXRot = Mth.rotLerp(partialTick, xRotO, xRot);
		float partialYRot = Mth.rotLerp(partialTick, yRotO, yRot);
		modelMatrix.translate(translation).rotateDeg(-partialYRot, Vec3f.Y_AXIS).rotateDeg(-partialXRot, Vec3f.X_AXIS).scale(scaleX, scaleY, scaleZ);
		
		return modelMatrix;
	}
	
	public static int getSign(double value) {
		return value > 0.0D ? 1 : -1;
	}
	
	public static Vec3 getVectorForRotation(float pitch, float yaw) {
		float f = pitch * (float) Math.PI / 180F;
		float f1 = -yaw * (float) Math.PI / 180F;
		float f2 = Mth.cos(f1);
		float f3 = Mth.sin(f1);
		float f4 = Mth.cos(f);
		float f5 = Mth.sin(f);
		
		return new Vec3(f3 * f4, -f5, f2 * f4);
	}
	
	public static float lerpBetween(float f1, float f2, float zero2one) {
		float f = 0;

		for (f = f2 - f1; f < -180.0F; f += 360.0F) {
		}

		while (f >= 180.0F) {
			f -= 360.0F;
		}

		return f1 + zero2one * f;
	}
	
	public static float rotlerp(float from, float to, float limit) {
		float f = Mth.wrapDegrees(to - from);
		
		if (f > limit) {
			f = limit;
		}
		
		if (f < -limit) {
			f = -limit;
		}
		
		float f1 = from + f;
		
		while (f1 >= 180.0F) {
			f1 -= 360.0F;
		}
		
		while (f1 <= -180.0F) {
			f1 += 360.0F;
		}
		
		return f1;
	}
	
	public static float wrapRadian(float pValue) {
		float maxRot = (float)Math.PI * 2.0F;
		float f = pValue % maxRot;
		
		if (f >= Math.PI) {
			f -= maxRot;
		}
		
		if (f < -Math.PI) {
			f += maxRot;
		}
		
		return f;
	}
	
	public static float findNearestRotation(float src, float rotation) {
		float diff = Math.abs(src - rotation);
		float idealRotation = rotation;
		int sign = Mth.sign(src - rotation);
		
		if (sign == 0) {
			return rotation;
		}
		
		while (true) {
			float next = idealRotation + sign * 360.0F;
			
			if (Math.abs(src - next) > diff) {
				return idealRotation;
			}
			
			idealRotation = next;
			diff = Math.abs(src - next);
		}
	}
	
	public static Vec3 getNearestVector(Vec3 from, Vec3... vectors) {
		double minLength = 1000000.0D;
		int index = 0;
		
		for (int i = 0; i < vectors.length; i++) {
			if (vectors[i] == null) {
				continue;
			}
			
			double distSqr = from.distanceToSqr(vectors[i]);
			
			if (distSqr < minLength) {
				minLength = distSqr;
				index = i;
			}
		}
		
		return vectors[index];
	}
	
	public static Vec3 getNearestVector(Vec3 from, List<Vec3> vectors) {
		return getNearestVector(from, vectors.toArray(new Vec3[0]));
	}
	
	private static final Matrix4f MATRIX4F = new Matrix4f();
	private static final Matrix3f MATRIX3F = new Matrix3f();
	
	public static void mulStack(PoseStack poseStack, OpenMatrix4f mat) {
		OpenMatrix4f.exportToMojangMatrix(mat, MATRIX4F);
		MATRIX3F.set(MATRIX4F);
		poseStack.mulPoseMatrix(MATRIX4F);
		poseStack.last().normal().mul(MATRIX3F);
	}
	
	public static double getAngleBetween(Vec3f a, Vec3f b) {
		Vec3f normA = Vec3f.normalize(a, null);
		Vec3f normB = Vec3f.normalize(b, null);
		
		double cos = (normA.x * normB.x + normA.y * normB.y + normA.z * normB.z);
		return Math.toDegrees(Math.acos(cos));
	}
	
	public static double getAngleBetween(Vec3 a, Vec3 b) {
		Vec3 normA = a.normalize();
		Vec3 normB = b.normalize();
		
		double cos = (normA.x * normB.x + normA.y * normB.y + normA.z * normB.z);
		return Math.toDegrees(Math.safeAcos(cos));
	}
	
	public static float getAngleBetween(Quaternionf a, Quaternionf b) {
		float dot = a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z;
		return 2.0F * (Math.safeAcos(MathUtils.getSign(dot) * b.w) - Math.safeAcos(a.w));
	}
	
	public static double getXRotOfVector(Vec3 vec) {
		Vec3 normalized = vec.normalize();
		return -(Math.atan2(normalized.y, (float)Math.sqrt(normalized.x * normalized.x + normalized.z * normalized.z)) * (180D / Math.PI));
	}
	
	public static double getYRotOfVector(Vec3 vec) {
		Vec3 normalized = vec.normalize();
		return Math.atan2(normalized.z, normalized.x) * (180D / Math.PI) - 90.0F;
	}
	
	private static Quaternionf getQuaternionFromMatrix(OpenMatrix4f mat) {
		Quaternionf quat = new Quaternionf(0, 0, 0, 1);
		quat.setFromUnnormalized(OpenMatrix4f.exportToMojangMatrix(mat.transpose(null)));
		return quat;
	}
	
	public static Vec3f lerpVector(Vec3f start, Vec3f end, float delta) {
		return lerpVector(start, end, delta, new Vec3f());
	}
	
	public static Vec3f lerpVector(Vec3f start, Vec3f end, float delta, Vec3f dest) {
		if (dest == null) {
			dest = new Vec3f();
		}
		
		dest.x = start.x + (end.x - start.x) * delta;
		dest.y = start.y + (end.y - start.y) * delta;
		dest.z = start.z + (end.z - start.z) * delta;
		
		return dest;
	}
	
	public static Vec3 lerpVector(Vec3 start, Vec3 end, float delta) {
		return new Vec3(start.x + (end.x - start.x) * delta, start.y + (end.y - start.y) * delta, start.z + (end.z - start.z) * delta);
	}
	
	public static Vec3 projectVector(Vec3 from, Vec3 to) {
		double dot = to.dot(from);
		double normalScale = 1.0D / ((to.x * to.x) + (to.y * to.y) + (to.z * to.z));
		
		return new Vec3(dot * to.x * normalScale, dot * to.y * normalScale, dot * to.z * normalScale);
	}
	
	public static Vec3f projectVector(Vec3f from, Vec3f to, Vec3f dest) {
		if (dest == null) {
			dest = new Vec3f();
		}
		
		float dot = Vec3f.dot(to, from);
		float normalScale = 1.0F / ((to.x * to.x) + (to.y * to.y) + (to.z * to.z));
		
		dest.x = dot * to.x * normalScale;
		dest.y = dot * to.y * normalScale;
		dest.z = dot * to.z * normalScale;
		
		return dest;
	}
	
	public static Quaternionf lerpQuaternion(Quaternionf from, Quaternionf to, float delta) {
		return lerpQuaternion(from, to, delta, null);
	}
	
	public static Quaternionf lerpQuaternion(Quaternionf from, Quaternionf to, float delta, Quaternionf dest) {
		if (dest == null) {
			dest = new Quaternionf();
		}
		
		float fromX = from.x();
		float fromY = from.y();
		float fromZ = from.z();
		float fromW = from.w();
		float toX = to.x();
		float toY = to.y();
		float toZ = to.z();
		float toW = to.w();
		float resultX;
		float resultY;
		float resultZ;
		float resultW;
		float dot = fromW * toW + fromX * toX + fromY * toY + fromZ * toZ;
		float blendI = 1.0F - delta;
		
		if (dot < 0.0F) {
			resultW = blendI * fromW + delta * -toW;
			resultX = blendI * fromX + delta * -toX;
			resultY = blendI * fromY + delta * -toY;
			resultZ = blendI * fromZ + delta * -toZ;
		} else {
			resultW = blendI * fromW + delta * toW;
			resultX = blendI * fromX + delta * toX;
			resultY = blendI * fromY + delta * toY;
			resultZ = blendI * fromZ + delta * toZ;
		}
		
		dest.set(resultX, resultY, resultZ, resultW);
		dest.normalize();
		
		return dest;
	}
	
	private static Vector3f getScaleVectorFromMatrix(OpenMatrix4f mat) {
		Vec3f a = new Vec3f(mat.m00, mat.m10, mat.m20);
		Vec3f b = new Vec3f(mat.m01, mat.m11, mat.m21);
		Vec3f c = new Vec3f(mat.m02, mat.m12, mat.m22);
		return new Vector3f(a.length(), b.length(), c.length());
	}
	
	public static <T> Set<Set<T>> getSubset(Collection<T> collection) {
		Set<Set<T>> subsets = new HashSet<> ();
		List<T> asList = new ArrayList<> (collection);
		createSubset(0, asList, new HashSet<> (), subsets);
		
		return subsets;
	}
	
	private static <T> void createSubset(int idx, List<T> elements, Set<T> parent, Set<Set<T>> subsets) {
		for (int i = idx; i < elements.size(); i++) {
			Set<T> subset = new HashSet<> (parent);
			subset.add(elements.get(i));
			subsets.add(subset);
			
			createSubset(i + 1, elements, subset, subsets);
		}
	}
	
	public static int getLeastAngleVectorIdx(Vec3f src, Vec3f... candidates) {
		int leastVectorIdx = -1;
		int current = 0;
		float maxDot = -10000.0F;
		
		for (Vec3f normzlizedVec : Stream.of(candidates).map((vec) -> vec.normalize()).collect(Collectors.toList())) {
			float dot = Vec3f.dot(src, normzlizedVec);
			
			if (maxDot < dot) {
				maxDot = dot;
				leastVectorIdx = current;
			}
			
			current++;
		}
		
		return leastVectorIdx;
	}
	
	public static boolean canBeSeen(Entity target, Entity watcher, double maxDistance) {
		if (target.level() != watcher.level()) {
			return false;
		}
		
		double sqr = maxDistance * maxDistance;
		Level level = target.level();
		Vec3 vec1 = watcher.getEyePosition();
		
		double height = target.getBoundingBox().maxY - target.getBoundingBox().minY;
		Vec3 vec2 = target.position().add(0.0D, height * 0.15D, 0.0D);
		Vec3 vec3 = target.position().add(0.0D, height * 0.5D, 0.0D);
		Vec3 vec4 = target.position().add(0.0D, height * 0.95D, 0.0D);
		
		return vec1.distanceToSqr(vec2) < sqr && level.clip(new ClipContext(vec1, vec2, ClipContext.Block.COLLIDER, ClipContext.Fluid.NONE, watcher)).getType() == HitResult.Type.MISS ||
				vec1.distanceToSqr(vec3) < sqr && level.clip(new ClipContext(vec1, vec3, ClipContext.Block.COLLIDER, ClipContext.Fluid.NONE, watcher)).getType() == HitResult.Type.MISS ||
				vec1.distanceToSqr(vec4) < sqr && level.clip(new ClipContext(vec1, vec4, ClipContext.Block.COLLIDER, ClipContext.Fluid.NONE, watcher)).getType() == HitResult.Type.MISS;
	}
	
	private MathUtils() {}
}
