package cz.fidentis.analyst.visitors.face;

import cz.fidentis.analyst.face.HumanFace;
import cz.fidentis.analyst.feature.FeaturePoint;
import cz.fidentis.analyst.feature.FeaturePointType;
import cz.fidentis.analyst.mesh.core.CornerTableRow;
import cz.fidentis.analyst.mesh.core.MeshFacet;
import cz.fidentis.analyst.mesh.core.MeshFacetImpl;
import cz.fidentis.analyst.mesh.core.MeshModel;
import cz.fidentis.analyst.mesh.core.MeshPointImpl;
import cz.fidentis.analyst.visitors.mesh.HausdorffDistance;
import static cz.fidentis.analyst.visitors.mesh.HausdorffDistance.Strategy.POINT_TO_POINT;
import java.io.File;
import java.io.IOException;
import java.util.ArrayList;
import java.util.Collections;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.Set;
import java.util.stream.Collectors;
import java.util.stream.IntStream;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.assertTrue;
import org.junit.jupiter.api.Test;

public class HausdorffDistancePrioritizedTest {
    
    private static final double DELTA = 1e-15;
    
    private final File boy = new File("src/test/resources/cz/fidentis/analyst/average_boy_17-20.obj");
    
    private static final int VERTICES_NUM = 12;
    
    private final MeshFacet facet1 = getTrivialFacet(VERTICES_NUM, 0.1, 0, 0, 0);
    private final MeshFacet facet2 = getTrivialFacet(VERTICES_NUM, -0.1, 0, 0, 0);
    private final MeshFacet facet3 = getTrivialFacet(VERTICES_NUM, 0, 0.1, 0, 0);
    private final MeshFacet facet4 = getTrivialFacet(VERTICES_NUM, 0, -0.1, 0, 0);
    private final MeshFacet facet5 = getTrivialFacet(VERTICES_NUM, 0, 0, 0.1, 0);
    private final MeshFacet facet6 = getTrivialFacet(VERTICES_NUM, 0, 0, -0.1, 0);
    private final List<MeshFacet> facets = List.of(facet1, facet2, facet3, facet4, facet5, facet6);
    
    private final List<MeshFacet> facets2 = List.of(getTrivialFacet(1, 0, 0, 0, 0));
    
    private final List<Double> prioritiesListFP1 = List.of(1d, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0d, 0d);
    private final double weightedDistanceFP1 = 0 + 0.09 + 0.16 + 0.21 + 0.24 + 0.25 + 0.24 + 0.21 + 0.16 + 0.09 + 0 + 0;
    private final double averageWeightedDistanceFP1 = weightedDistanceFP1 / prioritiesListFP1.stream()
            .mapToDouble(Double::doubleValue)
            .sum();
    
    private final Point3d featurePoint1 = new Point3d(0, 0, 0);
    private final FeaturePointType fpType1 = new FeaturePointType(0, null, null, null);
    private final Map<MeshFacet, List<Double>> expectedPrioritiesMapFP1 = Map.of(facet1, prioritiesListFP1,
            facet2, prioritiesListFP1,
            facet3, prioritiesListFP1,
            facet4, prioritiesListFP1,
            facet5, prioritiesListFP1,
            facet6, prioritiesListFP1);
    private final Map<MeshFacet, Double> expectedWeightedDistancesMapFP1 = Map.of(facet1, averageWeightedDistanceFP1,
            facet2, averageWeightedDistanceFP1,
            facet3, averageWeightedDistanceFP1,
            facet4, averageWeightedDistanceFP1,
            facet5, -averageWeightedDistanceFP1,
            facet6, averageWeightedDistanceFP1);
    private final Map<MeshFacet, Double> expectedAbsoluteWeightedDistancesMapFP1 = expectedWeightedDistancesMapFP1
            .entrySet()
            .stream()
            .collect(Collectors.toMap(entry -> entry.getKey(), entry -> Math.abs(entry.getValue())));
    
    private MeshFacet getTrivialFacet(int count, double xSpace, double ySpace, double zSpace, int offset) {
        final MeshFacet facet = new MeshFacetImpl();
        for (int i = offset; i < count + offset; i++) {
            facet.addVertex(new MeshPointImpl(new Point3d(xSpace * i, ySpace * i, zSpace * i),
                    new Vector3d(0, 0, 1),
                    new Vector3d()));
            facet.getCornerTable().addRow(new CornerTableRow(i - offset, -1));
        }

        return facet;
    }
    
    private HumanFace getHumanFace(List<MeshFacet> facets,
            List<Point3d> featurePoints,
            List<FeaturePointType> featurePointTypes) throws IOException {
        
        assertEquals(featurePoints.size(), featurePointTypes.size());
        
        final HumanFace face = new HumanFace(boy);
        
        final MeshModel model = new MeshModel();
        for (final MeshFacet facet: facets) {
            model.addFacet(facet);
        }
        face.setMeshModel(model);
        
        final List<FeaturePoint> featurePointsList = new ArrayList<>();
        for (int i = 0; i < featurePoints.size(); i++) {
            final Point3d fpCoordinates = featurePoints.get(i);
            featurePointsList.add(new FeaturePoint(
                    fpCoordinates.x,
                    fpCoordinates.y,
                    fpCoordinates.z,
                    featurePointTypes.get(i)));
        }
        face.setFeaturePoints(featurePointsList);
        
        return face;
    }
    
    private static Map<FeaturePointType, Double> zipListsToMap(List<FeaturePointType> fpTypes, List<Double> fpRadii) {
        assertEquals(fpTypes.size(), fpRadii.size());
        return IntStream.range(0, fpTypes.size())
                .boxed()
                .collect(Collectors.toMap(fpTypes::get, fpRadii::get));
    }
    
    protected void performTest(HumanFace face,
            Map<FeaturePointType, Map<MeshFacet, List<Double>>> expectedPriorities,
            Map<FeaturePointType, Map<MeshFacet, Double>> expectedFeaturePointWeights,
            Map<MeshFacet, List<Double>> expectedMergedPriorities,
            HausdorffDistancePrioritized visitor) {
        
        final Set<FeaturePointType> featurePointTypes = visitor.getFeaturePointTypes().keySet();
        final List<FeaturePoint> faceFeaturePoints = face.getFeaturePoints();
        featurePointTypes.forEach(fpType -> assertTrue(faceFeaturePoints.stream()
                .anyMatch(fPoint -> fPoint.getFeaturePointType().equals(fpType))));
                
        face.accept(visitor);
        
        final List<MeshFacet> faceFacets = face.getMeshModel().getFacets();
        
        // ---------------------------------------------------------------------
        // Test priorities
        // ---------------------------------------------------------------------
        final Map<HumanFace, Map<FeaturePointType, Map<MeshFacet, List<Double>>>> priorities =
                visitor.getPriorities();
        assertTrue(priorities.containsKey(face));
        final Map<FeaturePointType, Map<MeshFacet, List<Double>>> facePriorities =
                priorities.get(face);
        assertEquals(featurePointTypes.size(), facePriorities.size());
        
        for (final FeaturePointType fpType: featurePointTypes) {
            assertTrue(facePriorities.containsKey(fpType));
            
            final Map<MeshFacet, List<Double>> facePrioritiesMap = facePriorities.get(fpType);
            final Map<MeshFacet, List<Double>> expectedPrioritiesMap = expectedPriorities.get(fpType);
            assertEquals(faceFacets.size(), facePrioritiesMap.size());
            
            for (final MeshFacet facet: faceFacets) {
                assertTrue(facePrioritiesMap.containsKey(facet));
                
                final List<Double> facePrioritiesList = facePrioritiesMap.get(facet);
                final List<Double> expectedPrioritiesList = expectedPrioritiesMap.get(facet);
                assertEquals(expectedPrioritiesList.size(), facePrioritiesList.size());
                
                for (int i = 0; i < expectedPrioritiesList.size(); i++) {
                    assertEquals(expectedPrioritiesList.get(i), facePrioritiesList.get(i), DELTA);
                }
            }
        }
        
        // ---------------------------------------------------------------------
        // Test feature point weights
        // ---------------------------------------------------------------------
        final Map<HumanFace, Map<FeaturePointType, Map<MeshFacet, Double>>> featurePointWeights =
                visitor.getFeaturePointWeights();
        assertTrue(featurePointWeights.containsKey(face));
        final Map<FeaturePointType, Map<MeshFacet, Double>> faceFeaturePointWeights =
                featurePointWeights.get(face);
        assertEquals(featurePointTypes.size(), faceFeaturePointWeights.size());
        
        for (final FeaturePointType fpType: featurePointTypes) {
            assertTrue(faceFeaturePointWeights.containsKey(fpType));
            
            final Map<MeshFacet, Double> faceFPWeightsMap = faceFeaturePointWeights.get(fpType);
            final Map<MeshFacet, Double> expectedFPWeightsMap = expectedFeaturePointWeights.get(fpType);
            assertEquals(faceFacets.size(), faceFPWeightsMap.size());
            
            for (final MeshFacet facet: faceFacets) {
                assertTrue(faceFPWeightsMap.containsKey(facet));
                
                final double expectedWeightedDistance = expectedFPWeightsMap.get(facet);
                final double actualWeightedDistance = faceFPWeightsMap.get(facet);
                assertEquals(expectedWeightedDistance, actualWeightedDistance, DELTA);
            }
        }
        
        // ---------------------------------------------------------------------
        // Test merged priorities
        // ---------------------------------------------------------------------
        final Map<HumanFace, Map<MeshFacet, List<Double>>> mergedPriorities =
                visitor.getMergedPriorities();
        assertTrue(mergedPriorities.containsKey(face));
        final Map<MeshFacet, List<Double>> faceMergedPriorities =
                mergedPriorities.get(face);
        assertEquals(faceFacets.size(), faceMergedPriorities.size());
        
        for (final MeshFacet facet: faceFacets) {
            assertTrue(faceMergedPriorities.containsKey(facet));
            
            final List<Double> faceMergedPrioritiesList = faceMergedPriorities.get(facet);
            final List<Double> expectedMergedPrioritiesList = expectedMergedPriorities.get(facet);
            assertEquals(expectedMergedPrioritiesList.size(), faceMergedPrioritiesList.size());
            
            for (int i = 0; i < expectedMergedPrioritiesList.size(); i++) {
                assertEquals(expectedMergedPrioritiesList.get(i), faceMergedPrioritiesList.get(i), DELTA);
            }
        }
    }
    
    @Test
    public void singleFeaturePointTest() throws IOException {
        final List<Point3d> featurePoints = List.of(featurePoint1);
        final List<FeaturePointType> fpTypes = List.of(fpType1);
        final Map<FeaturePointType, Double> featurePointTypes = zipListsToMap(fpTypes, List.of(1d));
        
        final HumanFace face = getHumanFace(facets, featurePoints, fpTypes);
        
        final Map<FeaturePointType, Map<MeshFacet, List<Double>>> expectedPriorities = Map.of(fpType1, expectedPrioritiesMapFP1);
        final Map<FeaturePointType, Map<MeshFacet, Double>> expectedFeaturePointsWeights = Map.of(fpType1, expectedWeightedDistancesMapFP1);
        
        final Map<MeshFacet, List<Double>> expectedMergedPrioritiesMap = new HashMap<>();
        for (final MeshFacet facet: facets) {
            expectedMergedPrioritiesMap.put(facet, prioritiesListFP1);
        }
        
        final HausdorffDistancePrioritized visitor = new HausdorffDistancePrioritized(
                new HausdorffDistance(getHumanFace(facets2, List.of(), List.of()).getMeshModel(), POINT_TO_POINT, true, false, false),
                featurePointTypes);
        
        performTest(face, expectedPriorities, expectedFeaturePointsWeights, expectedMergedPrioritiesMap, visitor);
    }
    
    @Test
    public void twoFeaturePointsNoFacetIntersecTest() throws IOException {
        final List<Point3d> featurePoints = List.of(featurePoint1, new Point3d(3, 0, 0));
        final FeaturePointType fpType2 = new FeaturePointType(1, null, null, null);
        final List<FeaturePointType> fpTypes = List.of(fpType1, fpType2);
        final Map<FeaturePointType, Double> featurePointTypes = zipListsToMap(fpTypes, List.of(1d, 1d));
        
        final HumanFace face = getHumanFace(facets, featurePoints, fpTypes);
        
        final Map<MeshFacet, List<Double>> expectedPrioritiesMapFP2 = new HashMap<>();
        for (final MeshFacet facet: facets) {
            expectedPrioritiesMapFP2.put(facet, Collections.nCopies(VERTICES_NUM, 0d));
        }
        final Map<FeaturePointType, Map<MeshFacet, List<Double>>> expectedPriorities = Map.of(fpType1, expectedPrioritiesMapFP1,
                fpType2, expectedPrioritiesMapFP2);
        
        final Map<MeshFacet, Double> expectedWeightedDistancesMapFP2 = new HashMap<>();
        for (final MeshFacet facet: facets) {
            expectedWeightedDistancesMapFP2.put(facet, Double.NaN);
        }
        final Map<FeaturePointType, Map<MeshFacet, Double>> expectedFeaturePointsWeights = Map.of(fpType1, expectedAbsoluteWeightedDistancesMapFP1,
                fpType2, expectedWeightedDistancesMapFP2);
        
        final Map<MeshFacet, List<Double>> expectedMergedPrioritiesMap = new HashMap<>();
        for (final MeshFacet facet: facets) {
            expectedMergedPrioritiesMap.put(facet, prioritiesListFP1);
        }
        
        final HausdorffDistancePrioritized visitor = new HausdorffDistancePrioritized(
                new HausdorffDistance(getHumanFace(facets2, List.of(), List.of()).getMeshModel(), POINT_TO_POINT, false, false, false),
                featurePointTypes);
        
        performTest(face, expectedPriorities, expectedFeaturePointsWeights, expectedMergedPrioritiesMap, visitor);
    }
    
    @Test
    public void twoFeaturePointsSingleFacetIntersecTest() throws IOException {
        final List<Point3d> featurePoints = List.of(featurePoint1, new Point3d(1, 0, 0));
        final FeaturePointType fpType2 = new FeaturePointType(1, null, null, null);
        final List<FeaturePointType> fpTypes = List.of(fpType1, fpType2);
        final Map<FeaturePointType, Double> featurePointTypes = zipListsToMap(fpTypes, List.of(1d, 1d));
        
        final HumanFace face = getHumanFace(facets, featurePoints, fpTypes);
        
        final Map<MeshFacet, List<Double>> expectedPrioritiesMapFP2 = new HashMap<>();
        for (final MeshFacet facet: facets) {
            expectedPrioritiesMapFP2.put(facet, Collections.nCopies(VERTICES_NUM, 0d));
        }
        final List<Double> prioritiesList = List.of(0d, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1d, 0.9);
        expectedPrioritiesMapFP2.put(facet1, prioritiesList);
        final Map<FeaturePointType, Map<MeshFacet, List<Double>>> expectedPriorities = Map.of(fpType1, expectedPrioritiesMapFP1,
                fpType2, expectedPrioritiesMapFP2);
        
        final Map<MeshFacet, Double> expectedWeightedDistancesMapFP2 = new HashMap<>();
        for (final MeshFacet facet: facets) {
            expectedWeightedDistancesMapFP2.put(facet, Double.NaN);
        }
        expectedWeightedDistancesMapFP2.put(
                facet1,
                (0 + 0.01 + 0.04 + 0.09 + 0.16 + 0.25 + 0.36 + 0.49 + 0.64 + 0.81 + 1 + 0.99) / prioritiesList.stream()
                        .mapToDouble(Double::doubleValue)
                        .sum()
        );
        final Map<FeaturePointType, Map<MeshFacet, Double>> expectedFeaturePointsWeights = Map.of(fpType1, expectedWeightedDistancesMapFP1,
                fpType2, expectedWeightedDistancesMapFP2);
        
        final Map<MeshFacet, List<Double>> expectedMergedPrioritiesMap = new HashMap<>();
        for (final MeshFacet facet: facets) {
            expectedMergedPrioritiesMap.put(facet, prioritiesListFP1);
        }
        expectedMergedPrioritiesMap.put(facet1, List.of(1d, 0.9, 0.8, 0.7, 0.6, 0.5, 0.6, 0.7, 0.8, 0.9, 1d, 0.9));
        
        final HausdorffDistancePrioritized visitor = new HausdorffDistancePrioritized(
                new HausdorffDistance(getHumanFace(facets2, List.of(), List.of()).getMeshModel(), POINT_TO_POINT, true, false, false),
                featurePointTypes);
        
        performTest(face, expectedPriorities, expectedFeaturePointsWeights, expectedMergedPrioritiesMap, visitor);
    }
    
    @Test
    public void fourFeaturePointsSingleFacetIntersecTest() throws IOException {
        final List<Point3d> featurePoints = List.of(featurePoint1, new Point3d(1, 0, 0), new Point3d(0, 1, 0), new Point3d(0, 0, 1));
        final FeaturePointType fpType2 = new FeaturePointType(1, null, null, null);
        final FeaturePointType fpType3 = new FeaturePointType(2, null, null, null);
        final FeaturePointType fpType4 = new FeaturePointType(3, null, null, null);
        final List<FeaturePointType> fpTypes = List.of(fpType1, fpType2, fpType3, fpType4);
        final Map<FeaturePointType, Double> featurePointTypes = zipListsToMap(fpTypes, List.of(1d, 1d, 1d, 1d));
        
        final HumanFace face = getHumanFace(facets, featurePoints, fpTypes);
        
        final Map<MeshFacet, List<Double>> expectedPrioritiesMapFP2 = new HashMap<>();
        final Map<MeshFacet, List<Double>> expectedPrioritiesMapFP3 = new HashMap<>();
        final Map<MeshFacet, List<Double>> expectedPrioritiesMapFP4 = new HashMap<>();
        final List<Double> allZeroList = Collections.nCopies(VERTICES_NUM, 0d);
        for (final MeshFacet facet: facets) {
            expectedPrioritiesMapFP2.put(facet, allZeroList);
            expectedPrioritiesMapFP3.put(facet, allZeroList);
            expectedPrioritiesMapFP4.put(facet, allZeroList);
        }
        final List<Double> prioritiesList = List.of(0d, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1d, 0.9);
        expectedPrioritiesMapFP2.put(facet1, prioritiesList);
        expectedPrioritiesMapFP3.put(facet3, prioritiesList);
        expectedPrioritiesMapFP4.put(facet5, prioritiesList);
        final Map<FeaturePointType, Map<MeshFacet, List<Double>>> expectedPriorities = Map.of(fpType1, expectedPrioritiesMapFP1,
                fpType2, expectedPrioritiesMapFP2,
                fpType3, expectedPrioritiesMapFP3,
                fpType4, expectedPrioritiesMapFP4);
        
        final Map<MeshFacet, Double> expectedWeightedDistancesMapFP2 = new HashMap<>();
        final Map<MeshFacet, Double> expectedWeightedDistancesMapFP3 = new HashMap<>();
        final Map<MeshFacet, Double> expectedWeightedDistancesMapFP4 = new HashMap<>();
        for (final MeshFacet facet: facets) {
            expectedWeightedDistancesMapFP2.put(facet, Double.NaN);
            expectedWeightedDistancesMapFP3.put(facet, Double.NaN);
            expectedWeightedDistancesMapFP4.put(facet, Double.NaN);
        }
        final double weightedDistance = (0 + 0.01 + 0.04 + 0.09 + 0.16 + 0.25 + 0.36 + 0.49 + 0.64 + 0.81 + 1 + 0.99) / prioritiesList.stream()
                .mapToDouble(Double::doubleValue)
                .sum();
        expectedWeightedDistancesMapFP2.put(facet1, weightedDistance);
        expectedWeightedDistancesMapFP3.put(facet3, weightedDistance);
        expectedWeightedDistancesMapFP4.put(facet5, -weightedDistance);
        final Map<FeaturePointType, Map<MeshFacet, Double>> expectedFeaturePointsWeights = Map.of(fpType1, expectedWeightedDistancesMapFP1,
                fpType2, expectedWeightedDistancesMapFP2,
                fpType3, expectedWeightedDistancesMapFP3,
                fpType4, expectedWeightedDistancesMapFP4);
        
        final Map<MeshFacet, List<Double>> expectedMergedPrioritiesMap = new HashMap<>();
        for (final MeshFacet facet: facets) {
            expectedMergedPrioritiesMap.put(facet, prioritiesListFP1);
        }
        final List<Double> mergedPriorities = List.of(1d, 0.9, 0.8, 0.7, 0.6, 0.5, 0.6, 0.7, 0.8, 0.9, 1d, 0.9);
        expectedMergedPrioritiesMap.put(facet1, mergedPriorities);
        expectedMergedPrioritiesMap.put(facet3, mergedPriorities);
        expectedMergedPrioritiesMap.put(facet5, mergedPriorities);
        
        final HausdorffDistancePrioritized visitor = new HausdorffDistancePrioritized(
                new HausdorffDistance(getHumanFace(facets2, List.of(), List.of()).getMeshModel(), POINT_TO_POINT, true, false, false),
                featurePointTypes);
        
        performTest(face, expectedPriorities, expectedFeaturePointsWeights, expectedMergedPrioritiesMap, visitor);
    }
    
    @Test
    public void threeFeaturePointsMultiFacetIntersecTest() throws IOException {
        final List<Point3d> featurePoints = List.of(featurePoint1, new Point3d(0.999, 0.999, 0), new Point3d(0.999, -0.999, 0));
        final FeaturePointType fpType2 = new FeaturePointType(1, null, null, null);
        final FeaturePointType fpType3 = new FeaturePointType(2, null, null, null);
        final List<FeaturePointType> fpTypes = List.of(fpType1, fpType2, fpType3);
        final Map<FeaturePointType, Double> featurePointTypes = zipListsToMap(fpTypes, List.of(1d, 1d, 1d));
        
        final HumanFace face = getHumanFace(facets, featurePoints, fpTypes);
        
        final Map<MeshFacet, List<Double>> expectedPrioritiesMapFP2 = new HashMap<>();
        final Map<MeshFacet, List<Double>> expectedPrioritiesMapFP3 = new HashMap<>();
        final List<Double> allZeroList = Collections.nCopies(VERTICES_NUM, 0d);
        for (final MeshFacet facet: facets) {
            expectedPrioritiesMapFP2.put(facet, allZeroList);
            expectedPrioritiesMapFP3.put(facet, allZeroList);
        }
        
        final double vertexDistance = Math.sqrt(Math.pow(0.999 - 1, 2) + Math.pow(0.999 - 0, 2) + Math.pow(0 - 0, 2));
        final double vertexPriority = 1 - vertexDistance / 1;
        
        final List<Double> prioritiesList = List.of(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, vertexPriority, 0d);
        expectedPrioritiesMapFP2.put(facet1, prioritiesList);
        expectedPrioritiesMapFP2.put(facet3, prioritiesList);
        expectedPrioritiesMapFP3.put(facet1, prioritiesList);
        expectedPrioritiesMapFP3.put(facet4, prioritiesList);
        final Map<FeaturePointType, Map<MeshFacet, List<Double>>> expectedPriorities = Map.of(fpType1, expectedPrioritiesMapFP1,
                fpType2, expectedPrioritiesMapFP2,
                fpType3, expectedPrioritiesMapFP3);
        
        final Map<MeshFacet, Double> expectedWeightedDistancesMapFP2 = new HashMap<>();
        final Map<MeshFacet, Double> expectedWeightedDistancesMapFP3 = new HashMap<>();
        for (final MeshFacet facet: facets) {
            expectedWeightedDistancesMapFP2.put(facet, Double.NaN);
            expectedWeightedDistancesMapFP3.put(facet, Double.NaN);
        }
        final double weightedDistance = (0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 1 * vertexPriority + 0) / prioritiesList.stream()
                .mapToDouble(Double::doubleValue)
                .sum();
        expectedWeightedDistancesMapFP2.put(facet1, weightedDistance);
        expectedWeightedDistancesMapFP2.put(facet3, weightedDistance);
        expectedWeightedDistancesMapFP3.put(facet1, weightedDistance);
        expectedWeightedDistancesMapFP3.put(facet4, weightedDistance);
        final Map<FeaturePointType, Map<MeshFacet, Double>> expectedFeaturePointsWeights = Map.of(fpType1, expectedWeightedDistancesMapFP1,
                fpType2, expectedWeightedDistancesMapFP2,
                fpType3, expectedWeightedDistancesMapFP3);
        
        final Map<MeshFacet, List<Double>> expectedMergedPrioritiesMap = new HashMap<>();
        for (final MeshFacet facet: facets) {
            expectedMergedPrioritiesMap.put(facet, prioritiesListFP1);
        }
        final List<Double> mergedPriorities = List.of(1d, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, vertexPriority, 0d);
        expectedMergedPrioritiesMap.put(facet1, mergedPriorities);
        expectedMergedPrioritiesMap.put(facet3, mergedPriorities);
        expectedMergedPrioritiesMap.put(facet4, mergedPriorities);
        
        final HausdorffDistancePrioritized visitor = new HausdorffDistancePrioritized(
                new HausdorffDistance(getHumanFace(facets2, List.of(), List.of()).getMeshModel(), POINT_TO_POINT, true, false, false),
                featurePointTypes);
        
        performTest(face, expectedPriorities, expectedFeaturePointsWeights, expectedMergedPrioritiesMap, visitor);
    }
    
    @Test
    public void fourFeaturePointsSingleFacetIntersecParallelTest() throws IOException {
        final List<Point3d> featurePoints = List.of(featurePoint1, new Point3d(1, 0, 0), new Point3d(0, 1, 0), new Point3d(0, 0, 1));
        final FeaturePointType fpType2 = new FeaturePointType(1, null, null, null);
        final FeaturePointType fpType3 = new FeaturePointType(2, null, null, null);
        final FeaturePointType fpType4 = new FeaturePointType(3, null, null, null);
        final List<FeaturePointType> fpTypes = List.of(fpType1, fpType2, fpType3, fpType4);
        final Map<FeaturePointType, Double> featurePointTypes = zipListsToMap(fpTypes, List.of(1d, 1d, 1d, 1d));
        
        final HumanFace face = getHumanFace(facets, featurePoints, fpTypes);
        
        final Map<MeshFacet, List<Double>> expectedPrioritiesMapFP2 = new HashMap<>();
        final Map<MeshFacet, List<Double>> expectedPrioritiesMapFP3 = new HashMap<>();
        final Map<MeshFacet, List<Double>> expectedPrioritiesMapFP4 = new HashMap<>();
        final List<Double> allZeroList = Collections.nCopies(VERTICES_NUM, 0d);
        for (final MeshFacet facet: facets) {
            expectedPrioritiesMapFP2.put(facet, allZeroList);
            expectedPrioritiesMapFP3.put(facet, allZeroList);
            expectedPrioritiesMapFP4.put(facet, allZeroList);
        }
        final List<Double> prioritiesList = List.of(0d, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1d, 0.9);
        expectedPrioritiesMapFP2.put(facet1, prioritiesList);
        expectedPrioritiesMapFP3.put(facet3, prioritiesList);
        expectedPrioritiesMapFP4.put(facet5, prioritiesList);
        final Map<FeaturePointType, Map<MeshFacet, List<Double>>> expectedPriorities = Map.of(fpType1, expectedPrioritiesMapFP1,
                fpType2, expectedPrioritiesMapFP2,
                fpType3, expectedPrioritiesMapFP3,
                fpType4, expectedPrioritiesMapFP4);
        
        final Map<MeshFacet, Double> expectedWeightedDistancesMapFP2 = new HashMap<>();
        final Map<MeshFacet, Double> expectedWeightedDistancesMapFP3 = new HashMap<>();
        final Map<MeshFacet, Double> expectedWeightedDistancesMapFP4 = new HashMap<>();
        for (final MeshFacet facet: facets) {
            expectedWeightedDistancesMapFP2.put(facet, Double.NaN);
            expectedWeightedDistancesMapFP3.put(facet, Double.NaN);
            expectedWeightedDistancesMapFP4.put(facet, Double.NaN);
        }
        final double weightedDistance = (0 + 0.01 + 0.04 + 0.09 + 0.16 + 0.25 + 0.36 + 0.49 + 0.64 + 0.81 + 1 + 0.99) / prioritiesList.stream()
                .mapToDouble(Double::doubleValue)
                .sum();
        expectedWeightedDistancesMapFP2.put(facet1, weightedDistance);
        expectedWeightedDistancesMapFP3.put(facet3, weightedDistance);
        expectedWeightedDistancesMapFP4.put(facet5, -weightedDistance);
        final Map<FeaturePointType, Map<MeshFacet, Double>> expectedFeaturePointsWeights = Map.of(fpType1, expectedWeightedDistancesMapFP1,
                fpType2, expectedWeightedDistancesMapFP2,
                fpType3, expectedWeightedDistancesMapFP3,
                fpType4, expectedWeightedDistancesMapFP4);
        
        final Map<MeshFacet, List<Double>> expectedMergedPrioritiesMap = new HashMap<>();
        for (final MeshFacet facet: facets) {
            expectedMergedPrioritiesMap.put(facet, prioritiesListFP1);
        }
        final List<Double> mergedPriorities = List.of(1d, 0.9, 0.8, 0.7, 0.6, 0.5, 0.6, 0.7, 0.8, 0.9, 1d, 0.9);
        expectedMergedPrioritiesMap.put(facet1, mergedPriorities);
        expectedMergedPrioritiesMap.put(facet3, mergedPriorities);
        expectedMergedPrioritiesMap.put(facet5, mergedPriorities);
        
        final HausdorffDistancePrioritized visitor = new HausdorffDistancePrioritized(
                new HausdorffDistance(getHumanFace(facets2, List.of(), List.of()).getMeshModel(), POINT_TO_POINT, true, true, false),
                featurePointTypes);
        
        performTest(face, expectedPriorities, expectedFeaturePointsWeights, expectedMergedPrioritiesMap, visitor);
    }
}
