package cz.fidentis.analyst.visitors.kdtree;

import cz.fidentis.analyst.kdtree.KdNode;
import cz.fidentis.analyst.kdtree.KdTree;
import cz.fidentis.analyst.kdtree.KdTreeVisitor;
import cz.fidentis.analyst.mesh.core.MeshFacet;
import cz.fidentis.analyst.visitors.DistanceWithNearestPoints;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import javax.vecmath.Vector3d;
import java.util.HashMap;
import java.util.Map;
import java.util.Set;
import javax.vecmath.Point3d;

/**
 * This visitor finds the minimal distance between a given 3D point and triangular
 * meshes with vertices stored in a k-d tree.
 * <p>
 * The minimal distance is computed between the 3D point and triangles of the mesh facets,
 * i.e., the closest point at the triangle surface is found.  
 * This method is only a bit slower than the computation of the distance to mesh vertices, 
 * but more precise.
 * </p>
 * <p>
 * <b>Attention:</b> This visitor suppose that the k-d trees store mesh vertices 
 * (not triangle centroids).
 * </p>
 * <p>
 * <b>Use with caution!</b> This visitor finds the closest mesh vertex first and then 
 * explores triangles around the vertex to find the closest triangle. It is more
 * optimal than computing the distance to all triangles, but it works only for rather 
 * flat surfaces. It is okay for models of human faces where the "wrinkly" parts 
 * are considered noise and we aim to remove them from further processing anyway. 
 * </p>
 * <p>
 * This visitor is thread-safe, i.e., a single instance of the visitor can be used 
 * to inspect multiple k-d trees simultaneously.
 * </p>
 * 
 * @author Daniel Schramm
 */
public class KdTreeApproxDistanceToTriangles extends KdTreeVisitor implements DistanceWithNearestPoints {
    
    private double distance = Double.POSITIVE_INFINITY;
    private final Point3d point3d;
    private Map<MeshFacet, List<Point3d>> nearestPoints = new HashMap<>();
    private Map<MeshFacet, List<Integer>> nearestVertices = new HashMap<>();
    private final boolean checkOverlay;
    
    /**
     * Constructor.
     * 
     * @param point A 3D point from which distance is computed. Must not be {@code null}
     * @param checkOverlay If {@code true} and and closest point to the given 3D reference point 
     *        lies on the boundary of the mesh stored in the k-d tree, 
     *        then the reference point is considered "outside" of the mesh
     *        and the distance is set to infinity.
     * @throws IllegalArgumentException if some parameter is wrong
     */
    public KdTreeApproxDistanceToTriangles(Point3d point, boolean checkOverlay) {
        if (point == null) {
            throw new IllegalArgumentException("point");
        }
        this.point3d = point;
        this.checkOverlay = checkOverlay;
    }
    
    @Override
    public Map<MeshFacet, List<Point3d>> getNearestPoints() {
        return Collections.unmodifiableMap(nearestPoints);
    }

    @Override
    public double getDistance() {
        if (!checkOverlay || distance == Double.POSITIVE_INFINITY) {
            return distance;
        }
        
        for (MeshFacet f: nearestVertices.keySet()) {
            for (Integer i: nearestVertices.get(f)) {
                if (f.getOneRingNeighborhood(i).isBoundary()) {
                    return Double.POSITIVE_INFINITY;
                }
            }
        }
        
        return distance;
    }
    
    @Override
    public void visitKdTree(KdTree kdTree) {
        // find the closest nodes:
        final KdTreeClosestNode visitor = new KdTreeClosestNode(point3d);
        kdTree.accept(visitor);
        final Set<KdNode> closestNodes = visitor.getClosestNodes();
        
        // explore surrounding triangles:
        for (final KdNode node: closestNodes) { // For all nodes
            for (Map.Entry<MeshFacet, Integer> entry: node.getFacets().entrySet()) { // for all facets
                MeshFacet facet = entry.getKey();
                Integer vIndex = entry.getValue();
                
                final Point3d projection = facet.getClosestAdjacentPoint(point3d, vIndex);
                final Vector3d aux = new Vector3d(projection);
                aux.sub(point3d);
                final double dist = aux.length();
                
                synchronized (this) {
                    if (dist > distance) {
                        continue;
                    }
                    if (dist < distance) {
                        distance = dist;
                        nearestPoints.clear();                    
                    }
                    nearestPoints.computeIfAbsent(facet, meshFacet -> new ArrayList<>())
                            .add(projection);
                    nearestVertices.computeIfAbsent(facet, n -> new ArrayList<>())
                            .add(vIndex);
                }
            }
        }
    }
}
