package cz.fidentis.analyst.visitors.mesh;

import cz.fidentis.analyst.kdtree.KdTree;
import cz.fidentis.analyst.kdtree.KdTreeVisitor;
import cz.fidentis.analyst.mesh.MeshVisitor;
import cz.fidentis.analyst.mesh.core.MeshFacet;
import cz.fidentis.analyst.mesh.core.MeshModel;
import cz.fidentis.analyst.mesh.core.MeshPoint;
import cz.fidentis.analyst.visitors.Distance;
import cz.fidentis.analyst.visitors.DistanceWithNearestPoints;
import cz.fidentis.analyst.visitors.kdtree.KdTreeApproxDistanceToTriangles;
import cz.fidentis.analyst.visitors.kdtree.KdTreeDistance;
import cz.fidentis.analyst.visitors.kdtree.KdTreeDistanceToVertices;
import java.util.ArrayList;
import java.util.Collections;
import java.util.DoubleSummaryStatistics;
import java.util.HashMap;
import java.util.HashSet;
import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.concurrent.Callable;
import java.util.concurrent.ExecutionException;
import java.util.concurrent.ExecutorService;
import java.util.concurrent.Executors;
import java.util.concurrent.Future;
import javax.vecmath.Vector3d;
import java.util.logging.Level;
import java.util.logging.Logger;
import javax.vecmath.Point3d;

/**
 * Visitor for Hausdorff distance. 
 * This visitor is instantiated with a single k-d tree (either given as the input
 * parameter, or automatically created from the triangular mesh). 
 * When applied to other mesh facets, it computes Hausdorff distance from them to 
 * the instantiated k-d tree.
 * <p>
 * This visitor is thread-safe, i.e., a single instance of the visitor can be used 
 * to inspect multiple meshes simultaneously. 
 * </p>
 * <p>
 * The distance is computed either as absolute or relative. Absolute
 * represents Euclidean distance (all numbers are positive). On the contrary, 
 * relative distance considers orientation of the visited mesh (determined by its normal vectors)
 * and produces positive or negative distances depending on whether the primary
 * mesh is "in front of" or "behind" the given vertex.
 * </p>
 * <p>
 * Comparison of methods efficiency performed with scans of two human faces on 
 * Intel(R) Core(TM) i7-8565U CPU @ 1.80GHz, 8 CPU cores.
 * </p>
 * <p>
 * Hausdorff distance sequentially:
 * <table>
 * <tr><td>2443 msec</td><td>POINT_TO_POINT_DISTANCE_ONLY</td></tr>
 * <tr><td>2495 msec</td><td>POINT_TO_POINT</td></tr>
 * <tr><td>3430 msec</td><td>POINT_TO_TRIANGLE_APPROXIMATE</td></tr>
 * </table>
 * </p>
 * <p>
 * Hausdorff distance concurrently:
 * <table>
 * <tr><td>1782 msec</td><td>POINT_TO_POINT_DISTANCE_ONLY</td></tr>
 * <tr><td>2248 msec</td><td>POINT_TO_POINT</td></tr>
 * <tr><td>2574 msec</td><td>POINT_TO_TRIANGLE_APPROXIMATE</td></tr>
 * </table>
 * </p>
 * 
 * @author Daniel Schramm
 * @author Radek Oslejsek
 */
    public class HausdorffDistance extends MeshVisitor  {
    
    /**
     * Distance computation strategies.
     * @author Radek Oslejsek
     */
    public enum Strategy {
        
        /**
         * The fastest algorithm. Computes absolute distance between mesh vertices.
         * Relative distance is not supported, no closest points are returned.
         */
        POINT_TO_POINT_DISTANCE_ONLY,
        
        /**
         * A bit slower point-to-point algorithm. But supports both relative and absolute distances.
         * Moreover, the closest points are returned.
         * <p>
         * This algorithm can can produce "noise". If there are multiple closest points 
         * at the primary mesh, then it is not possible to choose which of them is correct (better). 
         * Therefore, the {@code Double.POSITIVE_INFINITY} value is set as the distance for this vertices.
         * If this situation happens with human faces, we would consider this part
         * of the face as noisy anyway, trying to avoid these parts from further processing.
         * </p>
         */
        POINT_TO_POINT,
        
        /**
         * The fast point-to-triangle distance strategy. Produces more precise results
         * than the {@code POINT_TO_POINT} strategy, but is a bit slower.
         * <p>
         * The algorithm supports both relative and absolute distances.
         * But is not geometrically correct. Can produce "noise"
         * in the sense that it may not found the really closest triangle on "wrinkly" surfaces. 
         * If this situation happens with human faces, we would consider this part
         * of the face as noisy anyway, trying to avoid these parts from further processing.
         * </p>
         */
        POINT_TO_TRIANGLE_APPROXIMATE
    }
    
    /**
     * Hausdorff distance for all inspected meshes.
     * Key = visited mesh facets. 
     * Value = Hausdorff distance of each vertex of the visited facet to the source facet.
     */
    private final Map<MeshFacet, List<Double>> distances = new HashMap<>();

    /**
     * The closest points for all inspected meshes (empty for the {@code POINT_TO_POINT_ABSOLUTE} strategy
     * Key = visited mesh facets.
     * Value = The nearest point of each vertex of the visited facet to the source facet.
     */
    private final Map<MeshFacet, List<Point3d>> nearestPoints = new HashMap<>();
    
    private final Strategy strategy;
    
    private final boolean relativeDist;
    
    private final boolean parallel;
    
    private final boolean crop;
    
    /**
     * The source triangular mesh (set of mesh facets) stored in k-d tree.
     */
    private final KdTree kdTree;
    
    /**
     * Constructor.
     * 
     * @param mainFacets Facets to which distance from other facets is to be computed. Must not be {@code null}
     * @param strategy Strategy of the computation of distance
     * @param relativeDistance If true, then the visitor calculates the relative distances with respect 
     * to the normal vectors of source facets (normal vectors have to present), 
     * i.e., we can get negative distances.
     * @param parallel If {@code true}, then the algorithm runs concurrently utilizing all CPU cores
     * @param crop If {@code true}, then only parts of the visited secondary faces that overlay the primary face are
     *        taken into account. Parts (vertices) that are out of the surface of the primary face are ignored 
     *        (their distance is set to {@code NaN}). 
     *        This feature makes the Hausdorff distance computation more symmetric.
     *        This optimization cannot be used for POINT_TO_POINT_DISTANCE_ONLY strategy
     * @throws IllegalArgumentException if some parameter is wrong
     */
    public HausdorffDistance(Set<MeshFacet> mainFacets, Strategy strategy, boolean relativeDistance, boolean parallel, boolean crop) {
        this.strategy = strategy;
        this.relativeDist = relativeDistance;
        this.parallel = parallel;
        if (mainFacets == null) {
            throw new IllegalArgumentException("mainFacets");
        }
        if (strategy == Strategy.POINT_TO_POINT_DISTANCE_ONLY && relativeDistance) {
            throw new IllegalArgumentException("POINT_TO_POINT_DISTANCE_ONLY strategy cannot be used with relative distance");
        }
        if (strategy == Strategy.POINT_TO_POINT_DISTANCE_ONLY && crop) {
            throw new IllegalArgumentException("POINT_TO_POINT_DISTANCE_ONLY strategy cannot be used with auto cutting parameter");
        }
        this.kdTree = new KdTree(new ArrayList<>(mainFacets));
        this.crop = crop;
    }

    /**
     * Constructor.
     * 
     * @param mainFacet Primary facet to which distance from others is to be computed. Must not be {@code null}
     * @param strategy Strategy of the computation of distance
     * @param relativeDistance If true, then the visitor calculates the relative distances with respect 
     * to the normal vectors of source facets (normal vectors have to present), 
     * i.e., we can get negative distances.
     * @param parallel If {@code true}, then the algorithm runs concurrently utilizing all CPU cores
     * @param crop If {@code true}, then only parts of the visited secondary faces that overlay the primary face are
     *        taken into account. Parts (vertices) that are out of the surface of the primary face are ignored 
     *        (their distance is set to {@code NaN}). 
     *        This feature makes the Hausdorff distance computation more symmetric.
     *        This optimization cannot be used for POINT_TO_POINT_DISTANCE_ONLY strategy
     * @throws IllegalArgumentException if some parameter is wrong
     */
    public HausdorffDistance(MeshFacet mainFacet, Strategy strategy, boolean relativeDistance, boolean parallel, boolean crop) {
        this(new HashSet<>(Collections.singleton(mainFacet)), strategy, relativeDistance, parallel, crop);
        if (mainFacet == null) {
            throw new IllegalArgumentException("mainFacet");
        }
    }

    /**
     * Constructor.
     * 
     * @param mainModel The mesh model with primary facets to which distance from 
     * others is to be computed. Must not be {@code null} or empty.
     * @param strategy Strategy of the computation of distance
     * @param relativeDistance If true, then the visitor calculates the relative distances with respect 
     * to the normal vectors of source facets (normal vectors have to present), 
     * i.e., we can get negative distances.
     * @param parallel If {@code true}, then the algorithm runs concurrently utilizing all CPU cores
     * @param crop If {@code true}, then only parts of the visited secondary faces that overlay the primary face are
     *        taken into account. Parts (vertices) that are out of the surface of the primary face are ignored 
     *        (their distance is set to {@code NaN}). 
     *        This feature makes the Hausdorff distance computation more symmetric.
     *        This optimization cannot be used for POINT_TO_POINT_DISTANCE_ONLY strategy
     * @throws IllegalArgumentException if some parameter is wrong
     */
    public HausdorffDistance(MeshModel mainModel, Strategy strategy, boolean relativeDistance, boolean parallel, boolean crop) {
        this(new HashSet<>(mainModel.getFacets()), strategy, relativeDistance, parallel, crop);
        if (mainModel.getFacets().isEmpty()) {
            throw new IllegalArgumentException("mainModel");
        }
    }
    
    /**
     * Constructor.
     * 
     * @param mainKdTree The KD tree to which distance from the visited facets is to be computed. Must not be {@code null}
     * @param strategy Strategy of the computation of distance
     * @param relativeDistance If true, then the visitor calculates the relative distances with respect 
     * to the normal vectors of source facets (normal vectors have to present), 
     * i.e., we can get negative distances.
     * @param parallel If {@code true}, then the algorithm runs concurrently utilizing all CPU cores
     * @param crop If {@code true}, then only parts of the visited secondary faces that overlay the primary face are
     *        taken into account. Parts (vertices) that are out of the surface of the primary face are ignored 
     *        (their distance is set to {@code NaN}). 
     *        This feature makes the Hausdorff distance computation more symmetric.
     *        This optimization cannot be used for POINT_TO_POINT_DISTANCE_ONLY strategy
     * @throws IllegalArgumentException if some parameter is wrong
     */
    public HausdorffDistance(KdTree mainKdTree, Strategy strategy, boolean relativeDistance, boolean parallel, boolean crop) {
        this.strategy = strategy;
        this.relativeDist = relativeDistance;
        this.parallel = parallel;
        if (mainKdTree == null) {
            throw new IllegalArgumentException("mainKdTree");
        }
        if (strategy == Strategy.POINT_TO_POINT_DISTANCE_ONLY && relativeDistance) {
            throw new IllegalArgumentException("POINT_TO_POINT_DISTANCE_ONLY strategy cannot be used with relative distance");
        }
        if (strategy == Strategy.POINT_TO_POINT_DISTANCE_ONLY && crop) {
            throw new IllegalArgumentException("POINT_TO_POINT_DISTANCE_ONLY strategy cannot be used with auto cutting parameter");
        }
        this.kdTree = mainKdTree;
        this.crop = crop;
    }
    
    /**
     * Returns Hausdorff distance of mesh facets to the source mesh facets.
     * 
     * Keys in the map contain mesh facets that were measured with the 
     * source facets. For each measured facet, a list of distances to the source 
     * facets is stored. The order of distances corresponds to the order of vertices
     * in the measured facet, i.e., the i-th value is the distance of the i-th vertex
     * of the measured facet.
     * 
     * @return Hausdorff distance for all points of all measured facets
     */
    public Map<MeshFacet, List<Double>> getDistances() {
        return Collections.unmodifiableMap(distances);
    }

    /**
     * Returns the nearest points of mesh facets to the source mesh facets.
     * 
     * Keys in the map contain mesh facets that were measured with the
     * source facets. For each measured facet, a list of the nearest points to the source
     * facets is stored. The order of  points corresponds to the order of vertices
     * in the measured facet, i.e., the i-th point is the nearest point of the i-th vertex
     * of the measured facet.
     *
     * @return The nearest points for all points of all measured facets
     */
    public Map<MeshFacet, List<Point3d>> getNearestPoints() {
        return Collections.unmodifiableMap(nearestPoints);
    }

    /**
     * Returns {@code true} if the distance was computed as relative 
     * (with possibly negative values). 
     * 
     * @return {@code true} if the distance is computed as relative, {@code false} otherwise.
     */
    public boolean relativeDistance() {
        return this.relativeDist;
    }
    
    public Strategy getStrategy() {
        return strategy;
    }
    
    /**
     * Returns {@code true} if the distance computation is parallel.
     * 
     * @return {@code true} if the distance computation is parallel.
     */
    public boolean inParallel() {
        return parallel;
    }
    
    /**
     * Return the KD tree to which distances are computed.
     * 
     * @return KD tree to which distances are computed
     */
    public KdTree getMainKdTree() {
        return kdTree;
    }
    
    /**
     * Returns statistics of (standard) Hausdorff distance.
     * @return statistics of (standard) Hausdorff distance.
     */
    public DoubleSummaryStatistics getStats() {
        return getDistances()
                .values()
                .stream()
                .flatMap(List::stream)
                .mapToDouble(Double::doubleValue)
                .filter(v -> Double.isFinite(v))
                .summaryStatistics();
    }
    
    @Override
    public void visitMeshFacet(MeshFacet comparedFacet) {
        final List<Double> distList = new ArrayList<>();
        final List<Point3d> nearestPointsList = new ArrayList<>();
        
        synchronized (this) {
            /*if (distances.containsKey(comparedFacet)) {
                return; // skip repeatedly visited facets
            }*/
            distances.put(comparedFacet, distList);
            if (getStrategy() != Strategy.POINT_TO_POINT_DISTANCE_ONLY) {
                nearestPoints.put(comparedFacet, nearestPointsList);
            }
        }

        final List<MeshPoint> vertices = comparedFacet.getVertices();
        
        ExecutorService executor = Executors.newFixedThreadPool(Runtime.getRuntime().availableProcessors());
        final List<Future<KdTreeVisitor>> results = new ArrayList<>(vertices.size());
        
        for (int i = 0; i < vertices.size(); i++) { // for each vertex of comparedFacet
            KdTreeVisitor visitor = instantiateVisitor(vertices.get(i).getPosition());
            if (inParallel() && visitor.isThreadSafe()) { // fork and continue
                results.add(executor.submit(new CallableVisitor(visitor)));
            } else { // process distance results immediately
                kdTree.accept(visitor); // compute the distance for i-th vertex or prepare the visitor for computation
                updateResults(visitor, vertices.get(i), comparedFacet);
            }
        }
            
        if (inParallel()) { // process asynchronous computation of distance
            executor.shutdown();
            while (!executor.isTerminated()){}
            try {
                int i = 0;
                for (Future<KdTreeVisitor> res: results) {
                    final KdTreeVisitor visitor = (KdTreeVisitor) res.get(); // waits until all computations are finished
                    updateResults(visitor, vertices.get(i), comparedFacet);
                    i++;
                }
            } catch (final InterruptedException | ExecutionException ex) {
                Logger.getLogger(HausdorffDistance.class.getName()).log(Level.SEVERE, null, ex);
            }
        }
    }
    
    protected KdTreeVisitor instantiateVisitor(Point3d inspectedPoint) {
        switch (getStrategy()) {
            case POINT_TO_POINT_DISTANCE_ONLY:
                return new KdTreeDistance(inspectedPoint);
            case POINT_TO_POINT:
                return new KdTreeDistanceToVertices(inspectedPoint, crop);
            default:
            case POINT_TO_TRIANGLE_APPROXIMATE:
                return new KdTreeApproxDistanceToTriangles(inspectedPoint, crop);
        }
    }

    protected void updateResults(KdTreeVisitor vis, MeshPoint point, MeshFacet facet) {
        Point3d closestV = null;
        
        if (vis instanceof DistanceWithNearestPoints) { // We have nearest points
            closestV = this.getClosestVertex((DistanceWithNearestPoints)vis);
            
            if (closestV == null) { // unable to get a single closest point
                distances.get(facet).add(Double.POSITIVE_INFINITY);
                nearestPoints.get(facet).add(null);
                return;
            }
        }

        double dist = ((Distance) vis).getDistance();
        int sign = 1;

        if (relativeDist) { // compute sign for relative distance
            Vector3d aux = new Vector3d(closestV);
            aux.sub(point.getPosition());
            sign = aux.dot(point.getNormal()) < 0 ? -1 : 1;
        }
        
        distances.get(facet).add(sign * dist);
        if (nearestPoints.containsKey(facet)) { // only strategies with the closest points are stored in the map
            nearestPoints.get(facet).add(closestV);
        }
    }
    
    /**
     * @param vis visitor
     * @return the only one existing closest vertex or {@code null}
     */
    protected Point3d getClosestVertex(DistanceWithNearestPoints vis) {
        if (vis.getNearestPoints().size() != 1) {
            return null; // somethig is wrong because there should be only my inspected facet
        }
        
        MeshFacet myInspectedFacet = vis.getNearestPoints().keySet().stream().findFirst().get();
        
        if (vis.getNearestPoints().get(myInspectedFacet).size() != 1) {
            return null; // multiple closest points; we don't know wich one to choose
        }
        
        // return the only one closest vertex
        return vis.getNearestPoints().get(myInspectedFacet).get(0);
    }
    
    /*
    protected boolean isOnBoundary(Point3d point3d) {
        KdTreeClosestNode vis = new KdTreeClosestNode(point3d);
        this.kdTree.accept(vis);
        for (KdNode node: vis.getClosestNodes()) {
            for (MeshFacet facet: node.getFacets().keySet()) {
                int vIndex = node.getFacets().get(facet);
                if (facet.getOneRingNeighborhood(vIndex).isBoundary()) {
                    return true; 
                }
            }
        }
        return false;
    }
    */
    
    /**
     * Helper call for asynchronous invocation of visitors.
     * 
     * @author Radek Oslejsek
     */
    private class CallableVisitor implements Callable<KdTreeVisitor> {
        private KdTreeVisitor vis;
        
        /**
         * Constructor.
         * @param vis visitor to be called asynchronously
         */
        CallableVisitor(KdTreeVisitor vis) {
            this.vis = vis;
        }
        
        @Override
        public KdTreeVisitor call() throws Exception {
            try {
                kdTree.accept(vis); 
                return vis;
            } catch(UnsupportedOperationException ex) {
                return null;
            }
        }
    };
}
