package com.example.fitbot.util.path;

import org.joml.Vector3f;

public class PathSegment {

    private final Vector3f start, end;
    private final double distance;

    /**
     * Constructor for creating a PathSegment of two lines, with the normal vector
     * pointing straight upwards relative to the line, with a curvature of 0.0.
     *
     * @param start The starting point of the line segment
     * @param end   The end point of the line segment.
     */
    public PathSegment(Vector3f start, Vector3f end) {
        this.start = start;
        this.end = end;
        this.distance = start.distance(end);
    }

    /**
     * Method that interpolates between the start and end points of the path segment,
     * depending on the curvature of the curve. If the curvature is unset, or set to 0
     * then this method will use linear interpolation. Otherwise, it will use a curve.
     *
     * @param t The interpolation value between 0 and 1.
     */
    public Vector3f interpolate(double t) {
        return new Vector3f(this.start)
                .lerp(this.end, (float) Math.min(1.0F, Math.max(0.0F, t)));
    }

    /**
     * Method for calculating the difference between the provided vector and the
     * path segment, depending on the normal vector and the curvature.
     * If the provided vector does not lie on the path segment, this method will return
     * the linear distance to the path.
     *
     * @param other The vector to calculate the difference to.
     * @return The difference between the vector and the path segment.
     */
    public double difference(Vector3f other) {

        if (this.distance == 0)
            return this.start.distance(other);

        double t = ((other.x - this.start.x) * (this.end.x - this.start.x) +
                (other.y - this.start.y) * (this.end.y - this.start.y) +
                (other.z - this.start.z) * (this.end.z - this.start.z)) / distance;

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

        return other.distance(new Vector3f(
                (float) (this.start.x + t * (this.end.x - this.start.x)),
                (float) (this.start.y + t * (this.end.y - this.start.y)),
                (float) (this.start.z + t * (this.end.z - this.start.z))
        ));
    }

    /**
     * Get the normal vector of the path segment.
     *
     * @return The normal vector of the path segment.
     */
    public Vector3f getStart() {
        return start;
    }

    /**
     * Get the end point of the path segment.
     *
     * @return The end point of the path segment.
     */
    public Vector3f getEnd() {
        return end;
    }

    /**
     * Method for returning the distance to the closest point on the path segment.
     *
     * @param reference The reference point to calculate the distance to.
     * @return The distance to the closest point on the path segment.
     */
    public double distance(Vector3f reference) {
        if (this.start.distanceSquared(reference) > this.end.distanceSquared(reference))
            return this.end.distance(reference);
        return this.start.distance(reference);
    }

    /**
     * Function for returning the closest path segment to a reference point.
     *
     * @param first          The first path segment to compare.
     * @param second         The second path segment to compare.
     * @param referencePoint The reference point to compare to.
     * @return The closest path segment to the reference point.
     */
    public static PathSegment closer(PathSegment first, PathSegment second, Vector3f referencePoint) {
        if (first.distance(referencePoint) < second.distance(referencePoint))
            return first;
        return second;
    }
}
