package com.example.fitbot.util.processing;

import java.util.Arrays;
import java.util.Comparator;

public class Vector3 {

    public double x, y, z;

    /**
     * Constructor for creating a new vector.
     *
     * @param x The X component of the vector.
     * @param y The Y component of the vector.
     * @param z The Z component of the vector.
     */
    public Vector3(double x, double y, double z) {
        this.x = x;
        this.y = y;
        this.z = z;
    }

    /**
     * Copy the vector.
     *
     * @return A new vector with the same values.
     */
    public Vector3 copy() {
        return new Vector3(this.x, this.y, this.z);
    }
    
    /**
     * Get the zero vector.
     * 
     * @return The zero vector.
     */
    public static Vector3 zero() {
        return new Vector3(0, 0, 0);
    }

    /**
     * Get the magnitude of the vector.
     *
     * @return The magnitude of the vector.
     */
    public double magnitude() {
        return Math.sqrt(this.x * this.x + this.y * this.y + this.z * this.z);
    }

    /**
     * Normalize the vector.
     *
     * @return The normalized vector.
     */
    public Vector3 normalize() {
        double mag = this.magnitude();
        if (mag == 0) throw new IllegalArgumentException("Cannot normalize the zero vector.");
        return new Vector3(this.x / mag, this.y / mag, this.z / mag);
    }

    /**
     * Subtract the vector from another vector.
     *
     * @param other The other vector to subtract.
     * @return The new vector.
     */
    public Vector3 subtract(Vector3 other) {
        return new Vector3(this.x - other.x, this.y - other.y, this.z - other.z);
    }

    /**
     * Add the vector to another vector.
     *
     * @param other The other vector to add.
     * @return The new vector.
     */
    public Vector3 add(Vector3 other) {
        return new Vector3(this.x + other.x, this.y + other.y, this.z + other.z);
    }

    /**
     * Multiply the vector by a scalar.
     *
     * @param scalar The scalar to multiply by.
     * @return The multiplied vector.
     */
    public Vector3 multiply(double scalar) {
        return new Vector3(this.x * scalar, this.y * scalar, this.z * scalar);
    }

    /**
     * Divide the vector by a scalar.
     *
     * @param scalar The scalar to divide by.
     * @return The divided vector.
     */
    public Vector3 divide(double scalar) {
        if (scalar == 0) throw new IllegalArgumentException("Cannot divide by zero.");
        return new Vector3(this.x / scalar, this.y / scalar, this.z / scalar);
    }

    /**
     * Negate the vector.
     *
     * @return The negated vector.
     */
    public Vector3 negate() {
        return new Vector3(-this.x, -this.y, -this.z);
    }

    /**
     * Rotate the vector around the X, Y, and Z axes.
     *
     * @param radX Rotation around the X axis in radians.
     * @param radY Rotation around the Y axis in radians.
     * @param radZ Rotation around the Z axis in radians.
     * @return The rotated vector.
     */
    public Vector3 rotate(double radX, double radY, double radZ) {
        double cosX = Math.cos(radX);
        double cosY = Math.cos(radY);
        double cosZ = Math.cos(radZ);
        double sinX = Math.sin(radX);
        double sinY = Math.sin(radY);
        double sinZ = Math.sin(radZ);
        double newX = x * cosY * cosZ + y * cosY * sinZ - z * sinY;
        double newY = x * (sinX * sinY * cosZ - cosX * sinZ) + y * (sinX * sinY * sinZ + cosX * cosZ) + z * sinX * cosY;
        double newZ = x * (cosX * sinY * cosZ + sinX * sinZ) + y * (cosX * sinY * sinZ - sinX * cosZ) + z * cosX * cosY;
        return new Vector3(newX, newY, newZ);
    }

    /**
     * Rotate the vector around the X, Y, and Z axes.
     *
     * @param rotation The rotation vector.
     * @return The rotated vector.
     */
    public Vector3 rotate(Vector3 rotation) {
        return rotate(rotation.x, rotation.y, rotation.z);
    }

    /**
     * Rotate the vector around the X axis.
     *
     * @param angle Rotation around the X axis in radians.
     * @return The rotated vector.
     */
    public Vector3 rotateX(double angle) {
        double sinTheta = Math.sin(angle);
        double cosTheta = Math.cos(angle);
        return new Vector3(
                x,
                y * cosTheta - z * sinTheta,
                y * sinTheta + z * cosTheta
        );
    }

    /**
     * Rotate the vector around the Y axis.
     *
     * @param angle Rotation around the Y axis in radians.
     * @return The rotated vector.
     */
    public Vector3 rotateY(double angle) {
        double sinTheta = Math.sin(angle);
        double cosTheta = Math.cos(angle);
        return new Vector3(
                x * cosTheta + z * sinTheta,
                y,
                -x * sinTheta + z * cosTheta
        );
    }

    /**
     * Rotate the vector around the Z axis.
     *
     * @param angle Rotation around the Z axis in radians.
     * @return The rotated vector.
     */
    public Vector3 rotateZ(double angle) {
        double sinTheta = Math.sin(angle);
        double cosTheta = Math.cos(angle);
        return new Vector3(
                x * cosTheta - y * sinTheta,
                x * sinTheta + y * cosTheta,
                z
        );
    }

    /**
     * Get the squared distance between this vector and another vector.
     *
     * @param compare The other vector.
     * @return The squared distance between the two vectors.
     */
    public double distanceSq(Vector3 compare) {
        return Math.pow(compare.x - x, 2) + Math.pow(compare.y - y, 2) + Math.pow(compare.z - z, 2);
    }

    /**
     * Get the distance between this vector and another vector.
     *
     * @param compare The other vector.
     * @return The distance between the two vectors.
     */
    public double distance(Vector3 compare) {
        return Math.sqrt(distanceSq(compare));
    }

    /**
     * Calculate the distance to a line defined by two points.
     *
     * @param lineStart The starting point of the line.
     * @param lineEnd The ending point of the line.
     * @return The distance to the line.
     */
    public double distanceToLine(Vector3 lineStart, Vector3 lineEnd) {
        double lineDistance = lineStart.distanceSq(lineEnd);
        if (lineDistance == 0)
            return this.distanceSq(lineStart);

        double t = ((this.x - lineStart.x) * (lineEnd.x - lineStart.x) +
                (this.y - lineStart.y) * (lineEnd.y - lineStart.y) +
                (this.z - lineStart.z) * (lineEnd.z - lineStart.z)) / lineDistance;

        t = Math.max(0, Math.min(1, t));

        return this.distanceSq(new Vector3(
                lineStart.x + t * (lineEnd.x - lineStart.x),
                lineStart.y + t * (lineEnd.y - lineStart.y),
                lineStart.z + t * (lineEnd.z - lineStart.z))
        );
    }

    /**
     * Retrieve the closest vector to this one given a list of vectors.
     *
     * @param vectors The list of vectors to compare.
     * @return The closest vector.
     */
    public Vector3 closest(Vector3 ... vectors) {
        return Arrays.stream(vectors).min(Comparator.comparingDouble(this::distanceSq)).orElse(null);
    }

    public Vector3 map(VectorMapFunction function) {
        return function.apply(this);
    }

    public interface VectorMapFunction {
        Vector3 apply(Vector3 vector);
    }

    @Override
    public String toString()
    {
        return "Vector3(" + this.x + ", " + this.y + ", " + this.z + ")";
    }
}
