package cz.fidentis.analyst.mesh.core;

import java.util.ArrayList;
import java.util.Iterator;
import java.util.List;
import java.util.NoSuchElementException;
import java.util.Objects;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

/**
 * Adapter for the corner table representing a single triangle of {@code MeshFacet}.
 * 
 * @author Natalia Bebjakova
 * @author Dominik Racek
 */

public class MeshTriangle implements Iterable<MeshPoint> { 
    
    public static final double EPS = 0.001; // tollerance
    
    private final MeshFacet facet;
    
    /**
     * Under which index is the corresponding vertex stored in the mesh facet
     */
    public final int index1;
    public final int index2;
    public final int index3;
    
    private Point3d voronoiPoint;
    
    /**
     * Creates new triangle
     * 
     * @param facet Mesh facet
     * @param i1 which index is the first vertex stored in the mesh facet
     * @param i2 which index is the second vertex stored in the mesh facet
     * @param i3 which index is the third vertex stored in the mesh facet
     */        
    public MeshTriangle(MeshFacet facet, int i1, int i2, int i3) {
        if (facet == null) {
            throw new IllegalArgumentException("facet");
        }
        this.index1 = i1;
        this.index2 = i2;
        this.index3 = i3;
        this.facet = facet;
    }
    
    public Point3d getVertex1() {
        return facet.getVertex(index1).getPosition();
    }
    
    public Point3d getVertex2() {
        return facet.getVertex(index2).getPosition();
    }
    
    public Point3d getVertex3() {
        return facet.getVertex(index3).getPosition();
    }
    
    public MeshPoint getPoint1() {
        return facet.getVertex(index1);
    }
    
    public MeshPoint getPoint2() {
        return facet.getVertex(index2);
    }
    
    public MeshPoint getPoint3() {
        return facet.getVertex(index3);
    }

    @Override
    public boolean equals(Object obj) {
        if(this == obj) {
            return true;
        }

        if(obj == null || obj.getClass()!= this.getClass()) {
            return false;
        }

        MeshTriangle mt = (MeshTriangle) obj;

        return ((index1 == mt.index1 || index1 == mt.index2 || index1 == mt.index3)
                && (index2 == mt.index1 || index2 == mt.index2 || index2 == mt.index3)
                && (index3 == mt.index1 || index3 == mt.index2 || index3 == mt.index3));
    }

    @Override
    public int hashCode() {
        return Objects.hash(index1, index2, index3);
    }

    
    /**
     * Computes and returns normalized normal vector from the vertices.
     * 
     * @return normalized normal vector from the vertices.
     */
    public Vector3d computeNormal() {
        Vector3d ab = new Vector3d(getVertex1());
        ab.sub(getVertex2());
        Vector3d ac = new Vector3d(getVertex1());
        ac.sub(getVertex3());
        Vector3d normal = new Vector3d();
        normal.cross(ab, ac);
        normal.normalize();
        return normal;
    }
    
    /**
     * Computes the point laying on the triangle which is closest to 
     * given 3D point. Return point is either one of the tringle's vertices,
     * a point laying on triangles edge, or a point laying on the plane of 
     * the triangle inside the triangle boundaries.
     * 
     * @param point 3D point
     * @return the closest point or {@code null} if the input parametr is missing
     */
    public Point3d getClosestPoint(Point3d point) {
        if (point == null) {
            return null;
        }
        Point3d projection = getProjectionToTrianglePlane(point);
        if (!isPointInTriangle(projection)) {
            projection = getProjectionToClosestEdge(projection);
        }
        return projection;
    }

    @Override
    public Iterator<MeshPoint> iterator() {
        return new Iterator<MeshPoint>() {
            private int counter = 0;
            
            @Override
            public boolean hasNext() {
                return counter < 3;
            }

            @Override
            public MeshPoint next() {
                if (!hasNext()) {
                    throw new NoSuchElementException();                    
                }
                switch (counter++) {
                    case 0:
                        return facet.getVertex(index1);
                    case 1:
                        return facet.getVertex(index2);
                    case 2:
                        return facet.getVertex(index3);
                    default:
                        return null;
                }                
            }
        };
    }
    
    /**
     * Return a center of circumcircle. This point represents the point
     * of Voronoi area used for Delaunay triangulation, for instenace.
     * 
     * @return the center of circumcircle
     */
    public Point3d getVoronoiPoint() {
        if (voronoiPoint != null) {
            return voronoiPoint;
        }
        
        MeshPoint vertex1 = facet.getVertex(index1);
        MeshPoint vertex2 = facet.getVertex(index2);
        MeshPoint vertex3 = facet.getVertex(index3);
        
        double a = subtractPosition(vertex2.getPosition(), vertex3).length();
        double b = subtractPosition(vertex3.getPosition(), vertex1).length();
        double c = subtractPosition(vertex2.getPosition(), vertex1).length();
        
        double d1 = a * a * (b * b + c * c - a * a);
        double d2 = b * b * (c * c + a * a - b * b);
        double d3 = c * c * (a * a + b * b - c * c);
        double dSum = d1 + d2 + d3;
        
        d1 /= dSum;
        d2 /= dSum;
        d3 /= dSum;
        
        Point3d v1Half = new Point3d(vertex2.getPosition());
        v1Half.add(vertex3.getPosition());
        v1Half.scale(0.5);
        
        Point3d v2Half = new Point3d(vertex1.getPosition());
        v1Half.add(vertex3.getPosition());
        v1Half.scale(0.5);
        
        Point3d v3Half = new Point3d(vertex2.getPosition());
        v1Half.add(vertex1.getPosition());
        v1Half.scale(0.5);

        //MeshPoint v1Half = (vertex2.addPosition(vertex3)).dividePosition(2);
        //MeshPoint v2Half = (vertex1.addPosition(vertex3)).dividePosition(2);
        //MeshPoint v3Half = (vertex2.addPosition(vertex1)).dividePosition(2);

        
        if (d1 < 0) {           
            Vector3d v = new Vector3d();
            v.cross(subtractPosition(v2Half, vertex3), subtractPosition(v1Half, vertex3));
            double v3Area = v.length() / 2.0;
            
            v.cross(subtractPosition(v3Half, vertex2), subtractPosition(v1Half, vertex2));
            double v2Area = v.length() / 2.0;
            
            v.cross(subtractPosition(v1Half, vertex1), subtractPosition(v3Half, vertex1));
            double v1Area = v.length() / 2.0;
            v.cross(subtractPosition(v1Half, vertex1), subtractPosition(v3Half, vertex1));
            v1Area += v.length() / 2.0;

            voronoiPoint = new Point3d(v1Area, v2Area, v3Area);
            return voronoiPoint;
        }
        if (d2 < 0) {
            Vector3d v = new Vector3d();
            v.cross(subtractPosition(v3Half, vertex1), subtractPosition(v2Half, vertex1));
            double v1Area = v.length() / 2.0;
            
            v.cross(subtractPosition(v1Half, vertex3), subtractPosition(v2Half, vertex3));
            double v3Area = v.length() / 2.0;
            
            v.cross(subtractPosition(v2Half, vertex2), subtractPosition(v1Half, vertex2));
            double v2Area = v.length() / 2.0;
            v.cross(subtractPosition(v2Half, vertex2), subtractPosition(v3Half, vertex2));
            v2Area += v.length() / 2.0;

            voronoiPoint = new Point3d(v1Area, v2Area, v3Area);
            return voronoiPoint;
        }
        if (d3 < 0) {
            Vector3d v = new Vector3d();
            v.cross(subtractPosition(v1Half, vertex2), subtractPosition(v3Half, vertex2));
            double v2Area = v.length() / 2.0;
            
            v.cross(subtractPosition(v2Half, vertex1), subtractPosition(v3Half, vertex1));
            double v1Area = v.length() / 2.0;
            
            v.cross(subtractPosition(v3Half, vertex3), subtractPosition(v2Half, vertex3));
            double v3Area = v.length() / 2.0;
            v.cross(subtractPosition(v3Half, vertex3), subtractPosition(v1Half, vertex3));
            v3Area += v.length() / 2.0;

            voronoiPoint = new Point3d(v1Area, v2Area, v3Area);
            return voronoiPoint;
        }
        
        Point3d aux1 = new Point3d(vertex1.getPosition());
        Point3d aux2 = new Point3d(vertex2.getPosition());
        Point3d aux3 = new Point3d(vertex3.getPosition());
        aux1.scale(d1);
        aux2.scale(d2);
        aux3.scale(d3);
        
        Point3d circumcenter = new Point3d(aux1);
        circumcenter.add(aux2);
        circumcenter.add(aux3);
        
        Vector3d v = new Vector3d();
        v.cross(subtractPosition(v2Half, vertex1), subtractPosition(circumcenter, vertex1));
        double v1Area = v.length() / 2.0;
        v.cross(subtractPosition(v3Half, vertex1), subtractPosition(circumcenter, vertex1));
        v1Area += v.length() / 2.0;
            
        v.cross(subtractPosition(v3Half, vertex2), subtractPosition(circumcenter, vertex2));
        double v2Area = v.length() / 2.0;
        v.cross(subtractPosition(v1Half, vertex2), subtractPosition(circumcenter, vertex2));
        v2Area += v.length() / 2.0;
            
        v.cross(subtractPosition(v1Half, vertex3), subtractPosition(circumcenter, vertex3));
        double v3Area = v.length() / 2.0;
        v.cross(subtractPosition(v2Half, vertex3), subtractPosition(circumcenter, vertex3));
        v3Area += v.length() / 2.0;
        
        voronoiPoint = new Point3d(v1Area, v2Area, v3Area);
        return voronoiPoint;
    }
    
    @Override
    public String toString() {
        String ret = "TRI {" + System.lineSeparator();
        for (MeshPoint p: this) {
            ret += "   " + p + System.lineSeparator();
        }
        ret += "}";
        return ret;
    }
    
    /**
     * Computes perpendicular projection from 3D point to the plane of triangle
     *
     * @param point   vertex from which the projection is created
     * @return projection to plane of triangle
     * @throws {@code NullPointerException} if the point is {@code null}
     */
    protected Point3d getProjectionToTrianglePlane(Point3d point) {
        Vector3d normal = computeNormal();
        Vector3d helperVector = new Vector3d(point);
        helperVector.sub(getVertex1());
        double dist = helperVector.dot(normal);

        Point3d projection = new Point3d(point);
        projection.scaleAdd(-dist, normal, projection);
        return projection;
    }
    
    /**
     * Checks if a point lying on the plane of the triangle lies also inside 
     * the triangle boundaries.
     *
     * @param point    Point located on the plane of the triangle
     * @return true if point is in triangle, false otherwise
     */
    protected boolean isPointInTriangle(Point3d point) {
        List<Vector3d> pointToVertices = new ArrayList<>();
        for (MeshPoint p: this) {
            Vector3d v = new Vector3d(p.getPosition());
            v.sub(point);
            pointToVertices.add(v);
        }
        
        double angleSum = 0;
        for (int i = 0; i < 3; i++) {
            angleSum += pointToVertices.get(i).angle(pointToVertices.get((i + 1) % 3));
        }
        angleSum -= 2 * Math.PI;
        
        return -EPS < angleSum && angleSum < EPS;
    }
    
    /**
     * Computes projection of a 3D point laying on the plane of the triangle 
     * to the nearest edge of the triangle
     *
     * @param point    point laying on the plane of the triangle
     * @return perpendicular projection to the nearest edge
     */
    protected Point3d getProjectionToClosestEdge(Point3d point) {
        double minDistance = Double.POSITIVE_INFINITY;
        Point3d closestProjection = null;

        Point3d projection = getProjectionToEdge(point, getVertex2(), getVertex1());
        Vector3d aux = new Vector3d(point);
        aux.sub(projection);
        double dist = aux.length();
        if (dist < minDistance) {
            minDistance = dist;
            closestProjection = projection;
        }
        
        projection = getProjectionToEdge(point, getVertex3(), getVertex2());
        aux.sub(point, projection);
        dist = aux.length();
        if (dist < minDistance) {
            minDistance = dist;
            closestProjection = projection;
        }
        
        projection = getProjectionToEdge(point, getVertex1(), getVertex3());
        aux.sub(point, projection);
        dist = aux.length();
        if (dist < minDistance) {
            minDistance = dist;
            closestProjection = projection;
        }
        
        return closestProjection;
    }
    
    /**
     * Computes projection to edge
     *
     * @param point Point laying on the plane of the triangle
     * @param v1 first vertex of edge
     * @param v2 second vertex of edge
     * @return projection to edge
     */
    protected Point3d getProjectionToEdge(Point3d point, Point3d v1, Point3d v2) {
        Vector3d ab = new Vector3d(v2);
        ab.sub(v1);
        Vector3d ap = new Vector3d(point);
        ap.sub(v1);

        double abLenSquared = ab.lengthSquared();
        
        double t = ab.dot(ap) / abLenSquared;
        Point3d projection = new Point3d(
                v1.x + t * ab.x,
                v1.y + t * ab.y,
                v1.z + t * ab.z);

        Vector3d projectionToEdgeVertex1 = new Vector3d(v1);
        projectionToEdgeVertex1.sub(projection);
        double len1 = projectionToEdgeVertex1.length();
        
        Vector3d projectionToEdgeVertex2 = new Vector3d(v2);
        projectionToEdgeVertex2.sub(projection);
        double len2 = projectionToEdgeVertex2.length();
        
        if (Math.abs((len1 + len2) - Math.sqrt(abLenSquared)) < EPS) {
            return projection;
        }

        if (len1 < len2) {
            return v1;
        }

        return v2;
    }
    
    protected Vector3d subtractPosition(Point3d p1, MeshPoint p2) {
        Vector3d e = new Vector3d(p1);
        e.sub(p2.getPosition());
        return e;
    }

    /**
     * Checks whether the triangle is intersected by a cutting plane
     *
     * @param normal normal defining the plane
     * @param d distance defining the plane
     * @return true if the triangle is intersected by the plane
     */
    public boolean checkIntersectionWithPlane(Vector3d normal, double d) {
        double p1 = (normal.x * getVertex1().x) + (normal.y * getVertex1().y) + (normal.z * getVertex1().z) + d;
        double p2 = (normal.x * getVertex2().x) + (normal.y * getVertex2().y) + (normal.z * getVertex2().z) + d;
        double p3 = (normal.x * getVertex3().x) + (normal.y * getVertex3().y) + (normal.z * getVertex3().z) + d;

        //If one point is on one side of the plane and the other on the other side
        return ((p1 <= 0 || p2 <= 0 || p3 <= 0) && (p1 >= 0 || p2 >= 0 || p3 >= 0));
    }

    /**
     * Selects the common points between two triangles
     *
     * @param other The other triangle
     * @return the common points of two triangles
     */
    public List<Point3d> getCommonPoints(MeshTriangle other) {
        List<Point3d> output = new ArrayList<>();

        if (getVertex1().equals(other.getVertex1()) || getVertex1().equals(other.getVertex2())
                || getVertex1().equals(other.getVertex3())) {
            output.add(getVertex1());
        }

        if (getVertex2().equals(other.getVertex1()) || getVertex2().equals(other.getVertex2())
                || getVertex2().equals(other.getVertex3())) {
            output.add(getVertex2());
        }

        if (getVertex3().equals(other.getVertex1()) || getVertex3().equals(other.getVertex2())
                || getVertex3().equals(other.getVertex3())) {
            output.add(getVertex3());
        }

        return output;
    }
}
