JuegoSim/Assets/_Project/Scripts/Code/Sockets/JointPlacementHandler.cs

using System.Collections;
using System.Collections.Generic;
//using Codice.Utils;
using UnityEngine;

public class JointPlacementHandler : MonoBehaviour
{
    private static JointPlacementHandler instance;

    [SerializeField] private float socketSearchRadius = 1f;
    [SerializeField] private LayerMask blockLayerMask;

    public static JointPlacementHandler getInstance() => instance;

    void Awake()
    {
        if (instance == null) instance = this;
        else Destroy(gameObject);
    }

    /// <summary>
    /// Finds all sockets located on the line segment between two specific sockets.
    /// Useful for identifying all "holes" a pivot passes through in a stack of links.
    /// </summary>
    public List<SocketPoint> FindAllSocketsAlignedBetween(SocketPoint start, SocketPoint end)
    {
        List<SocketPoint> alignedSockets = new List<SocketPoint>();

        Vector3 startPos = start.GetTransform().position;
        Vector3 endPos = end.GetTransform().position;

        Collider[] candidates;
        if (startPos == endPos)
        {
            // Use a Sphere search to find everything around the pos
            candidates = Physics.OverlapSphere(startPos, socketSearchRadius, blockLayerMask);
        }
        else
        {
            // Use a Capsule search to find everything in the "tunnel" between the two ends

            candidates = Physics.OverlapCapsule(startPos, endPos, socketSearchRadius, blockLayerMask);
        }
        foreach (Collider col in candidates)
        {
            // Don't detect sockets on the joint itself
            if (col.transform.root == transform.root) continue;

            SocketPoint[] socketsInBlock = col.GetComponentsInChildren<SocketPoint>();
            foreach (SocketPoint p in socketsInBlock)
            {
                // Skip the start and end points themselves
                if (p == start || p == end) continue;

                // 2. Mathematically check if the socket lies on the line segment
                if (IsPointOnSegment(startPos, endPos, p.GetTransform().position, 0.1f))
                {
                    alignedSockets.Add(p);
                }
            }
        }
        // remove start and end from it 
        if (alignedSockets.Contains(start))
            alignedSockets.Remove(start);

        if (alignedSockets.Contains(end))
            alignedSockets.Remove(end);

        return alignedSockets;
    }

    /// <summary>
    /// Helper to check if a point P is on the line segment AB within a certain tolerance
    /// </summary>
    private bool IsPointOnSegment(Vector3 A, Vector3 B, Vector3 P, float tolerance)
    {
        Vector3 ab = B - A;
        Vector3 ap = P - A;

        // Project point P onto the line AB to find its "position" along the line (t)
        float t = Vector3.Dot(ap, ab) / ab.sqrMagnitude;

        // Check if the projection is within the segment (between 0 and 1)
        if (t < 0 || t > 1) return false;

        // Calculate the distance from the point to the line
        Vector3 nearestPointOnSegment = A + t * ab;
        float distToLine = Vector3.Distance(P, nearestPointOnSegment);

        return distToLine <= tolerance;
    }

    /// <summary>
    /// Finds compatible sockets for a joint block to connect to
    /// </summary>
    public List<SocketPoint> FindTargetSocketsForJoint(JointBlock jointBlock, Vector3 searchPosition)
    {
        List<SocketPoint> validSockets = new List<SocketPoint>();

        SocketPoint end1 = jointBlock.GetSocketEnd1();
        SocketPoint end2 = jointBlock.GetSocketEnd2();

        if (end1 == null)
        {
            Debug.LogWarning("Joint block has no socket ends!");
            return validSockets;
        }

        // Find all nearby sockets
        Collider[] nearbyColliders = Physics.OverlapSphere(searchPosition, socketSearchRadius, blockLayerMask);
        List<SocketPoint> nearbySockets = new List<SocketPoint>();

        foreach (Collider col in nearbyColliders)
        {
            // Don't detect sockets on the joint itself
            if (col.transform.root == jointBlock.transform.root) continue;

            SocketPoint[] sockets = col.GetComponentsInChildren<SocketPoint>();
            foreach (SocketPoint socket in sockets)
            {
                if (socket.CanAccept(end1, true))
                {
                    nearbySockets.Add(socket);
                }
            }
        }

        if (nearbySockets.Count == 0)
        {
            Debug.Log("No compatible sockets found nearby");
            return validSockets;
        }

        SocketPoint nearestSocket = FindClosestSocket(searchPosition, nearbySockets);
        if (nearestSocket != null) validSockets.Add(nearestSocket);

        // For a two-ended joint (like a pivot/axle)
        if (end2 != null)
        {
            // Find the two closest compatible sockets
            SocketPoint socket2 = FindSocketAtJointEnd(end1, end1, end2);
            if (socket2 != null) validSockets.Add(socket2);
        }
        else
        {
            // Single-ended joint - just find closest socket
            SocketPoint closest = FindClosestSocket(end1.GetTransform().position, nearbySockets);
            if (closest != null) validSockets.Add(closest);
        }

        return validSockets;
    }

    private SocketPoint FindSocketAtJointEnd(SocketPoint firstSocket, SocketPoint end1, SocketPoint end2)
    {
        // 1. Calculate the required distance (the physical length of the joint)
        float jointLength = Vector3.Distance(end1.GetTransform().position, end2.GetTransform().position);

        // 2. Define the search direction (the negative normal of the first socket)
        Vector3 searchDirection = -firstSocket.GetNormal();

        // 3. Calculate exactly where the second end of the joint should land
        Vector3 targetPosition = firstSocket.GetTransform().position + (searchDirection * jointLength);

        // 4. Look for sockets near that target position
        Collider[] candidates = Physics.OverlapSphere(targetPosition, socketSearchRadius, blockLayerMask);

        SocketPoint bestMatch = null;
        float closestDistToTarget = float.MaxValue;

        foreach (Collider col in candidates)
        {
            // Don't connect the second end to the same block as the first end 
            // (Unless your design specifically allows 180-degree internal loops)
            if (col.transform.root == firstSocket.GetTransform().root) continue;

            SocketPoint[] sockets = col.GetComponentsInChildren<SocketPoint>();
            foreach (SocketPoint socket in sockets)
            {
                // Skip the first socket and occupied ones
                if (socket == firstSocket || socket.IsOccupied()) continue;

                // Check if this socket can accept the 'end2' type
                if (!socket.CanAccept(end2, true)) continue;

                // Optional: Check if the socket's normal is facing the right way 
                // (e.g., the second socket should face the SAME way as the first for a pass-through)
                float angleMatch = Vector3.Dot(socket.GetNormal(), firstSocket.GetNormal());
                if (angleMatch < 0.9f) continue; // Only accept if normals are roughly aligned

                float distToTarget = Vector3.Distance(socket.GetTransform().position, targetPosition);
                if (distToTarget < closestDistToTarget)
                {
                    closestDistToTarget = distToTarget;
                    bestMatch = socket;
                }
            }
        }

        return bestMatch;
    }

    private SocketPoint FindClosestSocket(Vector3 position, List<SocketPoint> sockets)
    {
        SocketPoint closest = null;
        float minDistance = float.MaxValue;

        foreach (SocketPoint socket in sockets)
        {
            float distance = Vector3.Distance(position, socket.GetTransform().position);
            if (distance < minDistance)
            {
                minDistance = distance;
                closest = socket;
            }
        }

        return closest;
    }
}