using System.Collections.Generic;
using System.Diagnostics;

namespace Color
{
    /// <summary>
    /// Point in a 3-Dimensional space.
    /// </summary>
    public struct Point
    {
        public double x;
        public double y;
        public double z;
    }


    public static class Color
    {
        /// <summary>
        /// Gets the euclidean distance between the two points without computing the square root.
        /// </summary>
        /// <param name="p1">First point.</param>
        /// <param name="p2">Second point.</param>
        /// <returns></returns>
        public static double EDist3DSquare(Point p1, Point p2)
        {
            double dx = p1.x - p2.x;
            double dy = p1.y - p2.y;
            double dz = p1.z - p2.z;

            return dx * dx + dy * dy + dz * dz;
        }


        /// <summary>
        /// Assigns to each point an identifier such that those points which distance is
        /// less or equal to the distance specified will have a different identifier.
        /// The identifier is an integer number greater or equal to 0.
        /// </summary>
        /// <param name="points">Points to map.</param>
        /// <param name="distance">The largest distance between two points to be considered neighbors.</param>
        /// <returns>Returns an array of identifiers.</returns>
        public static int[] Map(Point[] points, double distance)
        {
            double dist2 = distance * distance;
            var ids = new int[points.Length];

            // init identifiers with an invalid value
            for (int i = 0; i < ids.Length; i++)
                ids[i] = -1;

            // iterate over all points
            for (int i = 0; i < points.Length; i++)
            {
                int id = 0;

                // get neighbors identifier
                var colors = GetNeighborsID(points, ids, i, dist2);

                // compute the first available id
                while (colors.Contains(id))
                    id++;

                // assign the id
                ids[i] = id;
            }

            return ids;
        }


        /// <summary>
        /// Gets the neighbors color.
        /// </summary>
        /// <param name="points">Array of points.</param>
        /// <param name="ids">Array of identifiers.</param>
        /// <param name="idx">Index of the current point.</param>
        /// <param name="dist2">Neighbor distance (its square).</param>
        /// <returns>Returns a list of already assigned identifiers.</returns>
        private static List<int> GetNeighborsID(Point[] points, int[] ids, int idx, double dist2)
        {
            var colors = new List<int>();

            for (int i = 0; i < idx; i++)
            {
                Debug.Assert(ids[i] >= 0);

                // check if the i-point is a neighbor for idx-point
                if (EDist3DSquare(points[idx], points[i]) <= dist2)
                    colors.Add(ids[i]);
            }

            return colors;
        }
    }
}