// 
// Decompiled by Procyon v0.6.0
// 

package com.hypixel.hytale.builtin.hytalegenerator;

import java.util.Iterator;
import java.util.Comparator;
import it.unimi.dsi.fastutil.Pair;
import java.util.ArrayList;
import java.util.List;
import com.hypixel.hytale.math.vector.Vector2i;
import com.hypixel.hytale.math.vector.Vector2d;
import com.hypixel.hytale.math.vector.Vector3i;
import it.unimi.dsi.fastutil.doubles.DoubleObjectPair;
import com.hypixel.hytale.builtin.hytalegenerator.framework.math.Calculator;
import javax.annotation.Nonnull;
import com.hypixel.hytale.math.vector.Vector3d;

public class VectorUtil
{
    public static boolean areasOverlap(@Nonnull final Vector3d minA, @Nonnull final Vector3d maxA, @Nonnull final Vector3d minB, @Nonnull final Vector3d maxB) {
        return isAnyGreater(maxA, minB) && isAnySmaller(minA, maxB);
    }
    
    public static double distanceToSegment3d(@Nonnull final Vector3d point, @Nonnull final Vector3d p0, @Nonnull final Vector3d p1) {
        final Vector3d lineVec = p1.clone().addScaled(p0, -1.0);
        final Vector3d pointVec = point.clone().addScaled(p0, -1.0);
        final double lineLength = lineVec.length();
        final Vector3d lineUnitVec = lineVec.clone().setLength(1.0);
        final Vector3d pointVecScaled = pointVec.clone().scale(1.0 / lineLength);
        double t = lineUnitVec.dot(pointVecScaled);
        t = Calculator.clamp(0.0, t, 1.0);
        final Vector3d nearestPoint = lineVec.clone().scale(t);
        return nearestPoint.distanceTo(pointVec);
    }
    
    public static double distanceToLine3d(@Nonnull final Vector3d point, @Nonnull final Vector3d p0, @Nonnull final Vector3d p1) {
        final Vector3d lineVec = p1.clone().addScaled(p0, -1.0);
        final Vector3d pointVec = point.clone().addScaled(p0, -1.0);
        final double lineLength = lineVec.length();
        final Vector3d lineUnitVec = lineVec.clone().setLength(1.0);
        final Vector3d pointVecScaled = pointVec.clone().scale(1.0 / lineLength);
        final double t = lineUnitVec.dot(pointVecScaled);
        final Vector3d nearestPoint = lineVec.clone().scale(t);
        return nearestPoint.distanceTo(pointVec);
    }
    
    @Nonnull
    public static Vector3d nearestPointOnSegment3d(@Nonnull final Vector3d point, @Nonnull final Vector3d p0, @Nonnull final Vector3d p1) {
        final Vector3d lineVec = p1.clone().addScaled(p0, -1.0);
        final Vector3d pointVec = point.clone().addScaled(p0, -1.0);
        final double lineLength = lineVec.length();
        final Vector3d lineUnitVec = lineVec.clone().setLength(1.0);
        final Vector3d pointVecScaled = pointVec.clone().scale(1.0 / lineLength);
        double t = lineUnitVec.dot(pointVecScaled);
        t = Calculator.clamp(0.0, t, 1.0);
        final Vector3d nearestPoint = lineVec.clone().scale(t);
        return nearestPoint.add(p0);
    }
    
    @Nonnull
    public static Vector3d nearestPointOnLine3d(@Nonnull final Vector3d point, @Nonnull final Vector3d p0, @Nonnull final Vector3d p1) {
        final Vector3d lineVec = p1.clone().addScaled(p0, -1.0);
        final Vector3d pointVec = point.clone().addScaled(p0, -1.0);
        final double lineLength = lineVec.length();
        final Vector3d lineUnitVec = lineVec.clone().setLength(1.0);
        final Vector3d pointVecScaled = pointVec.clone().scale(1.0 / lineLength);
        final double t = lineUnitVec.dot(pointVecScaled);
        final Vector3d nearestPoint = lineVec.clone().scale(t);
        return nearestPoint.add(p0);
    }
    
    public static boolean[] shortestSegmentBetweenTwoSegments(@Nonnull final Vector3d a0, @Nonnull final Vector3d a1, @Nonnull final Vector3d b0, @Nonnull final Vector3d b1, final boolean clamp, @Nonnull final Vector3d p0Out, @Nonnull final Vector3d p1Out) {
        final boolean[] flags = new boolean[2];
        final Vector3d A = a1.clone().addScaled(a0, -1.0);
        final Vector3d B = b1.clone().addScaled(b0, -1.0);
        final double magA = A.length();
        final double magB = B.length();
        final Vector3d _A = A.clone().scale(1.0 / magA);
        final Vector3d _B = B.clone().scale(1.0 / magB);
        final Vector3d cross = _A.cross(_B);
        final double denom = Math.pow(cross.length(), 2.0);
        if (denom == 0.0) {
            flags[0] = true;
            final double d0 = _A.dot(b0.clone().addScaled(a0, -1.0));
            if (clamp) {
                final double d2 = _A.dot(b1.clone().addScaled(a0, -1.0));
                if (d0 <= 0.0 && d2 <= 0.0) {
                    if (Math.abs(d0) < Math.abs(d2)) {
                        p0Out.assign(a0);
                        p1Out.assign(b0);
                        flags[1] = true;
                        return flags;
                    }
                    p0Out.assign(a0);
                    p1Out.assign(b1);
                    flags[1] = true;
                    return flags;
                }
                else if (d0 >= magA && d2 >= magA) {
                    if (Math.abs(d0) < Math.abs(d2)) {
                        p0Out.assign(a1);
                        p1Out.assign(b0);
                        flags[1] = true;
                        return flags;
                    }
                    p0Out.assign(a1);
                    p1Out.assign(b1);
                    flags[1] = true;
                    return flags;
                }
            }
            return flags;
        }
        final Vector3d t = b0.clone().addScaled(a0, -1.0);
        final double detA = determinant(t, _B, cross);
        final double detB = determinant(t, _A, cross);
        final double t2 = detA / denom;
        final double t3 = detB / denom;
        Vector3d pA = _A.clone().scale(t2).add(a0);
        Vector3d pB = _B.clone().scale(t3).add(b0);
        if (clamp) {
            if (t2 < 0.0) {
                pA = a0.clone();
            }
            else if (t2 > magA) {
                pA = a1.clone();
            }
            if (t3 < 0.0) {
                pB = b0.clone();
            }
            else if (t3 > magB) {
                pB = b1.clone();
            }
            if (t2 < 0.0 || t2 > magA) {
                double dot = _B.dot(pA.clone().addScaled(b0, -1.0));
                if (dot < 0.0) {
                    dot = 0.0;
                }
                else if (dot > magB) {
                    dot = magB;
                }
                pB = b0.clone().add(_B.clone().scale(dot));
            }
            if (t3 < 0.0 || t3 > magA) {
                double dot = _A.dot(pB.clone().addScaled(a0, -1.0));
                if (dot < 0.0) {
                    dot = 0.0;
                }
                else if (dot > magA) {
                    dot = magA;
                }
                pA = a0.clone().add(_A.clone().scale(dot));
            }
        }
        p0Out.assign(pA);
        p1Out.assign(pB);
        flags[1] = true;
        return flags;
    }
    
    public static double shortestDistanceBetweenTwoSegments(@Nonnull final Vector3d a0, @Nonnull final Vector3d a1, @Nonnull final Vector3d b0, @Nonnull final Vector3d b1, final boolean clamp) {
        final Vector3d A = a1.clone().addScaled(a0, -1.0);
        final Vector3d B = b1.clone().addScaled(b0, -1.0);
        final double magA = A.length();
        final double magB = B.length();
        final Vector3d _A = A.clone().scale(1.0 / magA);
        final Vector3d _B = B.clone().scale(1.0 / magB);
        final Vector3d cross = _A.cross(_B);
        final double denom = Math.pow(cross.length(), 2.0);
        if (denom == 0.0) {
            final double d0 = _A.dot(b0.clone().addScaled(a0, -1.0));
            if (clamp) {
                final double d2 = _A.dot(b1.clone().addScaled(a0, -1.0));
                if (d0 <= 0.0 && d2 <= 0.0) {
                    if (Math.abs(d0) < Math.abs(d2)) {
                        return a0.distanceTo(b0);
                    }
                    return a0.distanceTo(b1);
                }
                else if (d0 >= magA && d2 >= magA) {
                    if (Math.abs(d0) < Math.abs(d2)) {
                        return a1.distanceTo(b0);
                    }
                    return a1.distanceTo(b1);
                }
            }
            return distanceToLine3d(a0, b0, b1);
        }
        final Vector3d t = b0.clone().addScaled(a0, -1.0);
        final double detA = determinant(t, _B, cross);
        final double detB = determinant(t, _A, cross);
        final double t2 = detA / denom;
        final double t3 = detB / denom;
        Vector3d pA = _A.clone().scale(t2).add(a0);
        Vector3d pB = _B.clone().scale(t3).add(b0);
        if (clamp) {
            if (t2 < 0.0) {
                pA = a0.clone();
            }
            else if (t2 > magA) {
                pA = a1.clone();
            }
            if (t3 < 0.0) {
                pB = b0.clone();
            }
            else if (t3 > magB) {
                pB = b1.clone();
            }
            if (t2 < 0.0 || t2 > magA) {
                double dot = _B.dot(pA.clone().addScaled(b0, -1.0));
                if (dot < 0.0) {
                    dot = 0.0;
                }
                else if (dot > magB) {
                    dot = magB;
                }
                pB = b0.clone().add(_B.clone().scale(dot));
            }
            if (t3 < 0.0 || t3 > magA) {
                double dot = _A.dot(pB.clone().addScaled(a0, -1.0));
                if (dot < 0.0) {
                    dot = 0.0;
                }
                else if (dot > magA) {
                    dot = magA;
                }
                pA = a0.clone().add(_A.clone().scale(dot));
            }
        }
        return pA.distanceTo(pB);
    }
    
    public static double determinant(@Nonnull final Vector3d v1, @Nonnull final Vector3d v2) {
        final Vector3d crossProduct = v1.cross(v2);
        return crossProduct.length();
    }
    
    public static double determinant(@Nonnull final Vector3d a, @Nonnull final Vector3d b, @Nonnull final Vector3d c) {
        double det = a.x * b.y * c.z + b.x * c.y * a.z + c.x * a.y * b.z;
        det -= a.z * b.y * c.x + b.z * c.y * a.x + c.z * a.y * b.x;
        return det;
    }
    
    @Nonnull
    public static DoubleObjectPair<Vector3d> distanceAndNearestPointOnSegment3d(@Nonnull final Vector3d point, @Nonnull final Vector3d p0, @Nonnull final Vector3d p1) {
        final Vector3d lineVec = p1.clone().addScaled(p0, -1.0);
        final Vector3d pointVec = point.clone().addScaled(p0, -1.0);
        final double lineLength = lineVec.length();
        final Vector3d lineUnitVec = lineVec.clone().setLength(1.0);
        final Vector3d pointVecScaled = pointVec.clone().scale(1.0 / lineLength);
        double t = lineUnitVec.dot(pointVecScaled);
        t = Calculator.clamp(0.0, t, 1.0);
        final Vector3d nearestPoint = lineVec.clone().scale(t);
        return DoubleObjectPair.of(nearestPoint.distanceTo(pointVec), nearestPoint.add(p0));
    }
    
    public static double angle(@Nonnull final Vector3d a, @Nonnull final Vector3d b) {
        final double top = a.x * b.x + a.y * b.y + a.z * b.z;
        final double bottomLeft = Math.sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
        final double bottomRight = Math.sqrt(b.x * b.x + b.y * b.y + b.z * b.z);
        return Math.acos(top / (bottomLeft * bottomRight));
    }
    
    public static void rotateAroundAxis(@Nonnull final Vector3d vec, @Nonnull final Vector3d axis, final double theta) {
        final Vector3d unitAxis = new Vector3d(axis);
        unitAxis.normalize();
        final double xPrime = unitAxis.x * (unitAxis.x * vec.x + unitAxis.y * vec.y + unitAxis.z * vec.z) * (1.0 - Math.cos(theta)) + vec.x * Math.cos(theta) + (-unitAxis.z * vec.y + unitAxis.y * vec.z) * Math.sin(theta);
        final double yPrime = unitAxis.y * (unitAxis.x * vec.x + unitAxis.y * vec.y + unitAxis.z * vec.z) * (1.0 - Math.cos(theta)) + vec.y * Math.cos(theta) + (unitAxis.z * vec.x - unitAxis.x * vec.z) * Math.sin(theta);
        final double zPrime = unitAxis.z * (unitAxis.x * vec.x + unitAxis.y * vec.y + unitAxis.z * vec.z) * (1.0 - Math.cos(theta)) + vec.z * Math.cos(theta) + (-unitAxis.y * vec.x + unitAxis.x * vec.y) * Math.sin(theta);
        vec.x = xPrime;
        vec.y = yPrime;
        vec.z = zPrime;
    }
    
    public static void rotateVectorByAxisAngle(@Nonnull final Vector3d vec, @Nonnull final Vector3d axis, final double angle) {
        final Vector3d crossProd = axis.cross(vec);
        final double cosAngle = Math.cos(angle);
        final double sinAngle = Math.sin(angle);
        final double x = vec.x * cosAngle + crossProd.x * sinAngle + axis.x * axis.dot(vec) * (1.0 - cosAngle);
        final double y = vec.y * cosAngle + crossProd.y * sinAngle + axis.y * axis.dot(vec) * (1.0 - cosAngle);
        final double z = vec.z * cosAngle + crossProd.z * sinAngle + axis.z * axis.dot(vec) * (1.0 - cosAngle);
        vec.x = x;
        vec.y = y;
        vec.z = z;
    }
    
    public static boolean isInside(@Nonnull final Vector3i point, @Nonnull final Vector3i min, @Nonnull final Vector3i max) {
        return point.x >= min.x && point.x < max.x && point.y >= min.y && point.y < max.y && point.z >= min.z && point.z < max.z;
    }
    
    public static boolean isInside(@Nonnull final Vector3d point, @Nonnull final Vector3d min, @Nonnull final Vector3d max) {
        return !isAnySmaller(point, min) && isSmaller(point, max);
    }
    
    public static boolean isAnySmaller(@Nonnull final Vector3d point, @Nonnull final Vector3d limit) {
        return point.x < limit.x || point.y < limit.y || point.z < limit.z;
    }
    
    public static boolean isSmaller(@Nonnull final Vector3d point, @Nonnull final Vector3d limit) {
        return point.x < limit.x && point.y < limit.y && point.z < limit.z;
    }
    
    public static boolean isAnyGreater(@Nonnull final Vector3d point, @Nonnull final Vector3d limit) {
        return point.x > limit.x || point.y > limit.y || point.z > limit.z;
    }
    
    public static boolean isAnySmaller(@Nonnull final Vector3i point, @Nonnull final Vector3i limit) {
        return point.x < limit.x || point.y < limit.y || point.z < limit.z;
    }
    
    public static boolean isAnyGreater(@Nonnull final Vector3i point, @Nonnull final Vector3i limit) {
        return point.x > limit.x || point.y > limit.y || point.z > limit.z;
    }
    
    public static boolean isInside(@Nonnull final Vector2d point, @Nonnull final Vector2d min, @Nonnull final Vector2d max) {
        return !isAnySmaller(point, min) && isSmaller(point, max);
    }
    
    public static boolean isAnySmaller(@Nonnull final Vector2d point, @Nonnull final Vector2d limit) {
        return point.x < limit.x || point.y < limit.y;
    }
    
    public static boolean isSmaller(@Nonnull final Vector2d point, @Nonnull final Vector2d limit) {
        return point.x < limit.x && point.y < limit.y;
    }
    
    public static boolean isAnyGreater(@Nonnull final Vector2d point, @Nonnull final Vector2d limit) {
        return point.x > limit.x || point.y > limit.y;
    }
    
    public static boolean isAnySmaller(@Nonnull final Vector2i point, @Nonnull final Vector2i limit) {
        return point.x < limit.x || point.y < limit.y;
    }
    
    public static boolean isSmaller(@Nonnull final Vector2i point, @Nonnull final Vector2i limit) {
        return point.x < limit.x && point.y < limit.y;
    }
    
    public static boolean isAnyGreater(@Nonnull final Vector2i point, @Nonnull final Vector2i limit) {
        return point.x > limit.x || point.y > limit.y;
    }
    
    @Nonnull
    public static Vector3i fromOperation(@Nonnull final Vector3i v1, @Nonnull final Vector3i v2, @Nonnull final BiOperation3i operation) {
        return new Vector3i(operation.run(v1.x, v2.x, Retriever.ofIndex(0)), operation.run(v1.y, v2.y, Retriever.ofIndex(1)), operation.run(v1.z, v2.z, Retriever.ofIndex(2)));
    }
    
    @Nonnull
    public static Vector3i fromOperation(@Nonnull final NakedOperation3i operation) {
        return new Vector3i(operation.run(Retriever.ofIndex(0)), operation.run(Retriever.ofIndex(1)), operation.run(Retriever.ofIndex(2)));
    }
    
    public static void bitShiftRight(final int shift, @Nonnull final Vector3i vector) {
        if (shift < 0) {
            throw new IllegalArgumentException("negative shift");
        }
        vector.x >>= shift;
        vector.y >>= shift;
        vector.z >>= shift;
        vector.dropHash();
    }
    
    public static void bitShiftLeft(final int shift, @Nonnull final Vector3i vector) {
        if (shift < 0) {
            throw new IllegalArgumentException("negative shift");
        }
        vector.x <<= shift;
        vector.y <<= shift;
        vector.z <<= shift;
        vector.dropHash();
    }
    
    @Nonnull
    public static List<Vector2i> orderByDistanceFrom(@Nonnull final Vector2i origin, @Nonnull final List<Vector2i> vectors) {
        final ArrayList<Pair<Double, Vector2i>> distances = new ArrayList<Pair<Double, Vector2i>>(vectors.size());
        for (int i = 0; i < vectors.size(); ++i) {
            final Vector2i vec = vectors.get(i);
            final double distance = Calculator.distance(vec.x, vec.y, origin.x, origin.y);
            distances.add(Pair.of(distance, vec));
        }
        distances.sort(Comparator.comparingDouble(Pair::first));
        final ArrayList<Vector2i> sorted = new ArrayList<Vector2i>(distances.size());
        for (final Pair<Double, Vector2i> pair : distances) {
            sorted.add(pair.second());
        }
        return sorted;
    }
    
    public static class Retriever
    {
        private int index;
        
        public Retriever(final int index) {
            this.index = index;
        }
        
        public int getIndex() {
            return this.index;
        }
        
        public int from(@Nonnull final Vector3i vec) {
            return switch (this.index) {
                case 0 -> vec.x;
                case 1 -> vec.y;
                case 2 -> vec.z;
                default -> throw new IllegalArgumentException();
            };
        }
        
        public int from(@Nonnull final Vector2i vec) {
            return switch (this.index) {
                case 0 -> vec.x;
                case 1 -> vec.y;
                default -> throw new IllegalArgumentException();
            };
        }
        
        public double from(@Nonnull final Vector3d vec) {
            return switch (this.index) {
                case 0 -> vec.x;
                case 1 -> vec.y;
                case 2 -> vec.z;
                default -> throw new IllegalArgumentException();
            };
        }
        
        public double from(@Nonnull final Vector2d vec) {
            return switch (this.index) {
                case 0 -> vec.x;
                case 1 -> vec.y;
                default -> throw new IllegalArgumentException();
            };
        }
        
        @Nonnull
        public static Retriever ofIndex(final int index) {
            return new Retriever(index);
        }
    }
    
    @FunctionalInterface
    public interface BiOperation3i
    {
        int run(final int p0, final int p1, @Nonnull final Retriever p2);
    }
    
    @FunctionalInterface
    public interface NakedOperation3i
    {
        int run(@Nonnull final Retriever p0);
    }
    
    @FunctionalInterface
    public interface Operation3i
    {
        int run(final int p0, @Nonnull final Retriever p1);
    }
}