package com.example.fitbot.util.processing;

import java.util.ArrayList;
import java.util.List;

public class GesturePath {

    // The vectors that make up the path.
    private final Vector3[] vectors;

    public GesturePath(Vector3[] vectors) {
        this.vectors = vectors;
    }

    /**
     * Get the error between an arbitrary path segment and the reference point.
     *
     * @param referencePoint The reference point to calculate the error of.
     * @return The error offset between the path and the reference point.
     */
    public double getError(Vector3 referencePoint) {
        // If there are no vectors, return 0.
        if ( vectors.length == 0)
            return 0;

        // If there's only one vector, return the distance to that vector.
        if ( vectors.length == 1)
            return vectors[0].distance(referencePoint);

        double distance = Double.MAX_VALUE;
        double currentDistSq, nextDistSq;
        int closestVectorIdx = 0;

        // Acquire two closest points to the reference point.
        for ( int i = 0; i < vectors.length - 1; i++) {
            currentDistSq = vectors[i].distanceSq(referencePoint);
            nextDistSq = vectors[i + 1].distanceSq(referencePoint);

            if ( currentDistSq < distance) {
                distance = currentDistSq;
                closestVectorIdx = i;
            } else if ( nextDistSq < distance) {
                distance = nextDistSq;
                closestVectorIdx = i + 1;
                i++; // Skip the next iteration; this point is already closer.
            }
        }

        // Calculate the error between the two closest points.
        Vector3 pointB = (closestVectorIdx == vectors.length - 1) ?
                vectors[closestVectorIdx - 1] : // If the closest point is the last point, use the 1 to last one
                (closestVectorIdx > 0 && // Find the closer point between the surrounding points.
                        (vectors[closestVectorIdx - 1].distanceSq(referencePoint) < vectors[closestVectorIdx + 1].distanceSq(referencePoint))) ?
                        vectors[closestVectorIdx - 1] :
                        vectors[closestVectorIdx + 1];

        return referencePoint.distanceToLine(vectors[closestVectorIdx], pointB);
    }

    // Builder class for the GesturePath object.
    public static class Builder {

        // List of vectors to add to the GesturePath object.
        private final List<Vector3> vectors;

        /**
         * Constructor for the Builder object.
         *
         * @param vectors The list of vectors to add.
         */
        public Builder(List<Vector3> vectors) {
            this.vectors = vectors;
        }

        /**
         * Default constructor for the Builder object.
         */
        public Builder() {
            this.vectors = new ArrayList<>();
        }

        /**
         * Adds a vector to the GesturePath object.
         *
         * @param vector The vector to add.
         * @return The Builder object.
         */
        public Builder addVector(Vector3 vector) {
            vectors.add(vector);
            return this;
        }

        /**
         * Builds the GesturePath object.
         *
         * @return The GesturePath object.
         */
        public GesturePath build() {
            return new GesturePath(vectors.toArray(new Vector3[0]));
        }

    }

}
