using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using CaeGlobals;
using CaeMesh.ContactSearchNamespace;

namespace CaeMesh
{
    public class ContactSearch
    {
        // Variables                                                                                                                
        private FeMesh _mesh;
        private FeMesh _geometry;
        private double[][] _nodes;
        private BoundingBox[][] _cellBoundingBoxes;
        private BoundingBox[][] _cellEdgeBoundingBoxes;
        private int[][] _edgeCellBaseCellIds;
        private bool _includeSelfContact;
        private GroupContactPairsByEnum _groupContactPairsBy;


        // Properties                                                                                                               
        public GroupContactPairsByEnum GroupContactPairsBy
        {
            get { return _groupContactPairsBy; }
            set { _groupContactPairsBy = value; }
        }


        // Constructors                                                                                                             
        public ContactSearch(FeMesh mesh, FeMesh geometry)
        {
            _mesh = mesh;
            _geometry = geometry;
            //
            if (_mesh != null && _mesh.Nodes != null)
            {
                _nodes = new double[_mesh.MaxNodeId + 1][];
                foreach (var nodeEntry in _mesh.Nodes) _nodes[nodeEntry.Key] = nodeEntry.Value.Coor;
            }
            //
            _includeSelfContact = false;
            _groupContactPairsBy = GroupContactPairsByEnum.None;
            // Get all edge cell base cell ids
            GetAllEdgeCellBaseCellIds();
        }


        // Methods                                                                                                                  
        public List<MasterSlaveItem> FindContactPairs(double distance, double angleDeg, GeometryFilterEnum filter,
                                                      bool tryToResolve)
        {
            if (_mesh == null) return null;
            // Bounding boxes for each cell
            ComputeCellBoundingBoxes(distance, filter.HasFlag(GeometryFilterEnum.IgnoreHidden));
            // Find all surfaces of the assembly
            ContactSurface[] contactSurfaces = GetAllContactSurfaces(distance, filter);
            // Find all surface pairs in contact
            double angleRad = angleDeg * Math.PI / 180;
            List<ContactSurface[]> contactSurfacePairs = new List<ContactSurface[]>();
            //
            for (int i = 0; i < contactSurfaces.Length; i++)
            {
                for (int j = i + 1; j < contactSurfaces.Length; j++)
                {
                    if (CheckSurfaceToSurfaceDistance(contactSurfaces[i], contactSurfaces[j], distance, angleRad))
                    {
                        contactSurfacePairs.Add(new ContactSurface[] { contactSurfaces[i], contactSurfaces[j] });
                    }
                }
            }
            // Group surface pairs
            if (_groupContactPairsBy == GroupContactPairsByEnum.None)
                return GroupContactPairsByNone(contactSurfacePairs, tryToResolve);
            else if (_groupContactPairsBy == GroupContactPairsByEnum.BySurfaceAngle)
                throw new NotSupportedException();
            else if (_groupContactPairsBy == GroupContactPairsByEnum.ByParts)
                return GroupContactPairsByParts(contactSurfacePairs, tryToResolve);
            else throw new NotSupportedException();
        }
        private void ComputeCellBoundingBoxes(double distance, bool ignoreHidden)
        {
            // Bounding boxes for each cell
            int[] cell;
            BoundingBox bb;
            _cellBoundingBoxes = new BoundingBox[_mesh.GetMaxPartId() + 1][];
            _cellEdgeBoundingBoxes = new BoundingBox[_mesh.GetMaxPartId() + 1][];
            //
            foreach (var partEntry in _mesh.Parts)
            {
                if (ignoreHidden && !partEntry.Value.Visible) continue;
                //
                _cellBoundingBoxes[partEntry.Value.PartId] = new BoundingBox[partEntry.Value.Visualization.Cells.Length];
                for (int i = 0; i < partEntry.Value.Visualization.Cells.Length; i++)
                {
                    cell = partEntry.Value.Visualization.Cells[i];
                    bb = new BoundingBox();
                    bb.IncludeFirstCoor(_nodes[cell[0]]);
                    bb.IncludeCoorFast(_nodes[cell[1]]);
                    bb.IncludeCoorFast(_nodes[cell[2]]);
                    if (cell.Length == 4 || cell.Length == 8) bb.IncludeCoorFast(_nodes[cell[3]]);
                    bb.Inflate(distance * 0.5);
                    //
                    _cellBoundingBoxes[partEntry.Value.PartId][i] = bb;
                }
                // Shell edge faces
                if (partEntry.Value.PartType == PartType.Shell)
                {
                    _cellEdgeBoundingBoxes[partEntry.Value.PartId]
                        = new BoundingBox[partEntry.Value.Visualization.EdgeCells.Length];
                    for (int i = 0; i < partEntry.Value.Visualization.EdgeCells.Length; i++)
                    {
                        cell = partEntry.Value.Visualization.EdgeCells[i];
                        bb = new BoundingBox();
                        bb.IncludeFirstCoor(_nodes[cell[0]]);
                        bb.IncludeCoorFast(_nodes[cell[1]]);
                        if (cell.Length == 3) bb.IncludeCoorFast(_nodes[cell[2]]);
                        bb.Inflate(distance * 0.5);
                        //
                        _cellEdgeBoundingBoxes[partEntry.Value.PartId][i] = bb;
                    }
                }
            }
        }
        private void GetAllEdgeCellBaseCellIds()
        {
            _edgeCellBaseCellIds = new int[_mesh.GetMaxPartId() + 1][];
            foreach (var partEntry in _mesh.Parts)
            {
                _edgeCellBaseCellIds[partEntry.Value.PartId] = partEntry.Value.Visualization.GetAllEdgeCellBaseCellIds();
            }
        }
        private ContactSurface[] GetAllContactSurfaces(double distance, GeometryFilterEnum filter)
        {
            HashSet<int> freeEdgeIds;
            ContactSurface contactSurface;
            List<ContactSurface> contactSurfacesList = new List<ContactSurface>();
            //
            foreach (var partEntry in _mesh.Parts)
            {
                if (filter.HasFlag(GeometryFilterEnum.IgnoreHidden) && !partEntry.Value.Visible) continue;
                //
                if (filter.HasFlag(GeometryFilterEnum.Solid) && partEntry.Value.PartType == PartType.Solid)
                {
                    // Solid faces
                    for (int i = 0; i < partEntry.Value.Visualization.FaceCount; i++)
                    {
                        contactSurfacesList.Add(new ContactSurface(_mesh, _nodes, partEntry.Value, i,
                                                                   GeometryType.SolidSurface, distance * 0.5));
                    }
                }
                else if (partEntry.Value.PartType == PartType.Shell)
                {
                    // Shell faces
                    if (filter.HasFlag(GeometryFilterEnum.Shell))
                    {
                        for (int i = 0; i < partEntry.Value.Visualization.FaceCount; i++)
                        {
                            // Front face
                            contactSurface = new ContactSurface(_mesh, _nodes, partEntry.Value, i,
                                                                GeometryType.ShellFrontSurface, distance * 0.5);
                            contactSurfacesList.Add(contactSurface);
                            // Back face
                            contactSurface = new ContactSurface(contactSurface);    // copy
                            contactSurface.ConvertToShellBackSurface();
                            contactSurfacesList.Add(contactSurface);
                        }
                    }
                    // Shell edge faces
                    if (filter.HasFlag(GeometryFilterEnum.ShellEdge))
                    {
                        freeEdgeIds = partEntry.Value.Visualization.GetFreeEdgeIds();
                        for (int i = 0; i < partEntry.Value.Visualization.EdgeCount; i++)
                        {
                            contactSurface = new ContactSurface(_mesh, _nodes, partEntry.Value, i,
                                                                GeometryType.ShellEdgeSurface, distance * 0.5);
                            // Fnd internal edges on shells
                            if (!freeEdgeIds.Contains(i)) contactSurface.Internal = true;
                            contactSurfacesList.Add(contactSurface);
                        }
                    }
                }
            }
            //
            ContactSurface[] contactSurfaces = contactSurfacesList.ToArray();
            // Find internal surfaces on compound parts
            for (int i = 0; i < contactSurfaces.Length; i++)
            {
                for (int j = i + 1; j < contactSurfaces.Length; j++)
                {
                    if (contactSurfaces[i].GeometryType == contactSurfaces[j].GeometryType && // skip front and back shell
                        contactSurfaces[i].NodeIds.Count() == contactSurfaces[j].NodeIds.Count() &&
                        contactSurfaces[i].BoundingBox.Intersects(contactSurfaces[j].BoundingBox) &&
                        contactSurfaces[i].NodeIds.Union(contactSurfaces[j].NodeIds).Count() == contactSurfaces[i].NodeIds.Count)
                    {
                        contactSurfaces[i].Internal = true;
                        contactSurfaces[j].Internal = true;
                    }
                }
            }
            return contactSurfaces;
        }
        private bool CheckSurfaceToSurfaceDistance(ContactSurface cs1, ContactSurface cs2, double distance, double angleRad)
        {
            if (cs1.Internal || cs2.Internal) return false;
            if (!_includeSelfContact && cs1.Part == cs2.Part) return false;
            if (!cs1.BoundingBox.Intersects(cs2.BoundingBox)) return false;
            //
            BoundingBox bb1;
            BoundingBox bb2;
            BoundingBox[] bbs1;
            BoundingBox[] bbs2;
            BoundingBox intersection = cs1.BoundingBox.GetIntersection(cs2.BoundingBox);
            //
            int[] cell1;
            int[] cell2;
            int[] cell1Ids;
            int[] cell2Ids;
            double[] cellNorm = new double[3];
            double[] edgeCellNorm = new double[3];
            double[][] t1 = new double[3][];
            double[][] t2 = new double[3][];
            t1[2] = new double[3];
            t2[2] = new double[3];
            //
            bool edgeSurface1 = cs1.GeometryType == GeometryType.ShellEdgeSurface;
            bool edgeSurface2 = cs2.GeometryType == GeometryType.ShellEdgeSurface;
            bool allowPenetration = (edgeSurface1 && edgeSurface2) ||
                                    (cs1.GeometryType == GeometryType.SolidSurface &&
                                     cs1.GeometryType == GeometryType.SolidSurface);
            bool onlyInternal = (cs1.GeometryType == GeometryType.SolidSurface ||
                                 cs1.GeometryType == GeometryType.ShellFrontSurface ||
                                 cs1.GeometryType == GeometryType.ShellBackSurface) &&
                                (cs2.GeometryType == GeometryType.SolidSurface ||
                                 cs2.GeometryType == GeometryType.ShellFrontSurface ||
                                 cs2.GeometryType == GeometryType.ShellBackSurface);

            onlyInternal = false;
            // Use face cell ids or edge cell ids
            if (edgeSurface1)
            {
                bbs1 = _cellEdgeBoundingBoxes[cs1.Part.PartId];
                cell1Ids = cs1.Part.Visualization.EdgeCellIdsByEdge[cs1.Id];
            }
            else
            {
                bbs1 = _cellBoundingBoxes[cs1.Part.PartId];
                cell1Ids = cs1.Part.Visualization.CellIdsByFace[cs1.Id];
            }
            //
            foreach (int cell1Id in cell1Ids)
            {
                bb1 = bbs1[cell1Id];
                //
                if (bb1.Intersects(intersection))
                {
                    // Use face cell ids or edge cell ids
                    if (edgeSurface2)
                    {
                        bbs2 = _cellEdgeBoundingBoxes[cs2.Part.PartId];
                        cell2Ids = cs2.Part.Visualization.EdgeCellIdsByEdge[cs2.Id];
                    }
                    else
                    {
                        bbs2 = _cellBoundingBoxes[cs2.Part.PartId];
                        cell2Ids = cs2.Part.Visualization.CellIdsByFace[cs2.Id];
                    }
                    //
                    foreach (int cell2Id in cell2Ids)
                    {
                        bb2 = bbs2[cell2Id];
                        //
                        if (bb1.Intersects(bb2))
                        {
                            cell1 = cs1.GetCell(cell1Id);   // reverse cell ids for shell back faces or get shell edge cell
                            cell2 = cs2.GetCell(cell2Id);   // reverse cell ids for shell back faces or get shell edge cell
                            // Cell 1 triangle 1
                            t1[0] = _nodes[cell1[0]];
                            t1[1] = _nodes[cell1[1]];
                            if (!edgeSurface1) t1[2] = _nodes[cell1[2]];
                            // Cell 2 triangle 1
                            t2[0] = _nodes[cell2[0]];
                            t2[1] = _nodes[cell2[1]];
                            if (!edgeSurface2) t2[2] = _nodes[cell2[2]];
                            // 1. triangle is edge segment - create a triangle out of it
                            if (edgeSurface1)
                            {
                                cs1.GetEdgeCellNormal(cell1Id, _edgeCellBaseCellIds[cs1.Part.PartId][cell1Id],
                                                      ref cellNorm, ref edgeCellNorm);
                                Geometry.VpVxS(ref t1[2], t1[1], cellNorm, 0.001 * distance);
                            }
                            // 2. triangle is edge segment - create a triangle out of it
                            if (edgeSurface2)
                            {
                                cs2.GetEdgeCellNormal(cell2Id, _edgeCellBaseCellIds[cs2.Part.PartId][cell2Id],
                                                      ref cellNorm, ref edgeCellNorm);
                                Geometry.VpVxS(ref t2[2], t2[1], cellNorm, 0.001 * distance);
                            }
                            if (CheckTriangleToTriangleDistance(t1, t2, distance, angleRad, allowPenetration, onlyInternal))
                                return true;
                            // Cell 2 is a rectangle
                            if (cell2.Length == 4 || cell2.Length == 8)
                            {
                                // Cell 2 triangle 2
                                t2[0] = _nodes[cell2[0]];
                                t2[1] = _nodes[cell2[2]];
                                t2[2] = _nodes[cell2[3]];
                                //
                                if (CheckTriangleToTriangleDistance(t1, t2, distance, angleRad, allowPenetration, onlyInternal))
                                    return true;
                            }
                            // Cell 1 is a rectangle
                            if (cell1.Length == 4 || cell1.Length == 8)
                            {
                                // Cell 1 triangle 2
                                t1[0] = _nodes[cell1[0]];
                                t1[1] = _nodes[cell1[2]];
                                t1[2] = _nodes[cell1[3]];
                                //
                                if (CheckTriangleToTriangleDistance(t1, t2, distance, angleRad, allowPenetration, onlyInternal))
                                    return true;
                                // Cell 2 triangle 1 again
                                if (cell2.Length == 4 || cell2.Length == 8)
                                {
                                    t2[0] = _nodes[cell2[0]];
                                    t2[1] = _nodes[cell2[1]];
                                    t2[2] = _nodes[cell2[2]];
                                    //
                                    if (CheckTriangleToTriangleDistance(t1, t2, distance, angleRad, allowPenetration, onlyInternal))
                                        return true;
                                }
                            }
                        }
                    }
                }
            }
            return false;
        }
        private bool CheckTriangleToTriangleDistance(double[][] t1, double[][] t2, double distance, double angleRad,
                                                     bool penetrationPossible, bool onlyInternal)
        {
            double dist;
            double ang;
            //
            double[] a = new double[3];
            double[] b = new double[3];
            double[] c = new double[3];
            double[] d = new double[3];
            double[] n1 = new double[3];
            double[] n2 = new double[3];
            // Triangle 1
            Geometry.VmV(ref a, t1[1], t1[0]);
            Geometry.VmV(ref b, t1[2], t1[0]);
            Geometry.VcrossV(ref n1, a, b);
            Geometry.VxS(ref n1, n1, 1 / Math.Sqrt(Geometry.VdotV(n1, n1)));
            // Triangle 2
            Geometry.VmV(ref c, t2[1], t2[0]);
            Geometry.VmV(ref d, t2[2], t2[0]);
            Geometry.VcrossV(ref n2, c, d);
            Geometry.VxS(ref n2, n2, 1 / Math.Sqrt(Geometry.VdotV(n2, n2)));
            //
            double vDotV = Geometry.Clamp(Geometry.VdotV(n1, n2), -1, 1);
            // 180° - the normal point in the opposite directions
            ang = Math.PI - Math.Acos(vDotV);
            if (ang < angleRad)
            {
                // Closest points on triangles t1 and t2 are p adn q, respectively
                double[] p = new double[3];
                double[] q = new double[3];
                double[] pq = new double[3];
                //
                dist = Geometry.TriDist(ref p, ref q, t1, t2, onlyInternal);
                //
                if (dist < distance)
                {
                    // Check the orientation of the triangle normal
                    if (dist > 0)
                    {
                        double ang1;
                        double ang2;
                        // pq is a vector from p to q
                        Geometry.VmV(ref pq, q, p);
                        Geometry.Vnorm(ref pq, pq);
                        ang1 = Geometry.VdotV(pq, n1);
                        if (!penetrationPossible && ang1 < 0) return false;     // negative value means n1 points away from q
                        ang2 = Geometry.VdotV(pq, n2);
                        if (!penetrationPossible && ang2 > 0) return false;     // positive value means n2 points away from p
                        // Check that the closest triangle points are one above the other
                        // The angle between the closest direction vector and normal must be small < 5° = 0.995
                        if (Math.Abs(ang1) < 0.995 && Math.Abs(ang2) < 0.995) return false;
                    }
                    // Shrink the triangles and try again - the triangles should not overlap only over an vertex or an edge
                    double[][] t1s = Geometry.ShrinkTriangle(t1, 3 * distance);
                    double[][] t2s = Geometry.ShrinkTriangle(t2, 3 * distance);
                    dist = Geometry.TriDist(ref p, ref q, t1s, t2s, false);
                    if (dist > distance)
                      return false;
                    //
                    return true;
                }
            }
            return false;
        }
        //
        private List<MasterSlaveItem> GroupContactPairsByNone(List<ContactSurface[]> contactSurfacePairs, bool tryToResolve)
        {
            List<MasterSlaveItem> masterSlaveItems = new List<MasterSlaveItem>();
            //
            foreach (var csp in contactSurfacePairs)
            {
                masterSlaveItems.Add(new MasterSlaveItem(csp[0].Part.Name, csp[1].Part.Name,
                                                         new HashSet<int>() { csp[0].GeometryId },
                                                         new HashSet<int>() { csp[1].GeometryId }));
            }
            //
            ContactGraph contactGraph = new ContactGraph();
            contactGraph.AddMasterSlaveItems(masterSlaveItems, _mesh);
            masterSlaveItems = contactGraph.GetMasterSlaveItems(tryToResolve);
            //
            return masterSlaveItems;
        }
        private List<MasterSlaveItem> GroupContactPairsByParts(List<ContactSurface[]> contactSurfacePairs, bool tryToResolve)
        {
            int i;
            int j;
            int iType;
            int jType;
            int[] key;
            MasterSlaveItem masterSlaveItem;
            CompareIntArray comparer = new CompareIntArray();
            Dictionary<int[], MasterSlaveItem> partKeyMasterSlaveItems = new Dictionary<int[], MasterSlaveItem>(comparer);
            //
            Dictionary<int, int> partIds = GetPartIdsMergedByCompounds();
            // Merge by part Id
            foreach (var csp in contactSurfacePairs)
            {
                if (partIds[csp[0].Part.PartId] < partIds[csp[1].Part.PartId])
                {
                    i = 0;
                    j = 1;
                }
                else
                {
                    i = 1;
                    j = 0;
                }
                //
                if (csp[i].GeometryType == GeometryType.ShellEdgeSurface) iType = 1;
                else iType = 0;
                if (csp[j].GeometryType == GeometryType.ShellEdgeSurface) jType = 1;
                else jType = 0;
                //
                key = new int[] { partIds[csp[i].Part.PartId], iType, partIds[csp[j].Part.PartId], jType };
                if (partKeyMasterSlaveItems.TryGetValue(key, out masterSlaveItem))
                {
                    masterSlaveItem.MasterGeometryIds.Add(csp[i].GeometryId);
                    masterSlaveItem.SlaveGeometryIds.Add(csp[j].GeometryId);
                }
                else
                {
                    masterSlaveItem = new MasterSlaveItem(csp[i].Part.Name, csp[j].Part.Name,
                                                          new HashSet<int>(), new HashSet<int>());
                    masterSlaveItem.MasterGeometryIds.Add(csp[i].GeometryId);
                    masterSlaveItem.SlaveGeometryIds.Add(csp[j].GeometryId);
                    partKeyMasterSlaveItems.Add(key, masterSlaveItem);
                }
            }
            //
            ContactGraph contactGraph = new ContactGraph();
            contactGraph.AddMasterSlaveItems(partKeyMasterSlaveItems.Values, _mesh);
            List<MasterSlaveItem> masterSlaveItems = contactGraph.GetMasterSlaveItems(tryToResolve);
            //
            return masterSlaveItems;
          }
        private Dictionary<int,int> GetPartIdsMergedByCompounds()
        {
            int maxPartId = 0;
            Dictionary<int, int> partIds = new Dictionary<int, int>();
            foreach (var partEntry in _mesh.Parts)
            {
                partIds.Add(partEntry.Value.PartId, partEntry.Value.PartId);
                if (partEntry.Value.PartId > maxPartId) maxPartId = partEntry.Value.PartId;
            }
            //
            if (_geometry != null && _geometry.Parts != null)
            {
                BasePart part;
                foreach (var geometryPartEntry in _geometry.Parts)
                {
                    if (geometryPartEntry.Value is CompoundGeometryPart cgp)
                    {
                        foreach (string subPartName in cgp.SubPartNames)
                        {
                            if (_mesh.Parts.TryGetValue(subPartName, out part)) partIds[part.PartId] = maxPartId;
                        }
                        maxPartId++;
                    }
                }
            }
            //
            return partIds;
        }
    }
}