I150/srcs/RobotNet10/Commons/RobotNet10.GlobalPathPlanner/Forklift/TStructure.cs

using RobotNet10.GlobalPathPlanner.Model;

namespace RobotNet10.GlobalPathPlanner.Forklift;

public enum TStructureDirection
{
    NODE1_NODE2_NODE3,
    NODE1_NODE3_NODE2,
    NODE2_NODE1_NODE3,
    NODE2_NODE3_NODE1,
    NODE3_NODE2_NODE1,
    NODE3_NODE1_NODE2,
}

public class TStructure
{
    public Guid Id { get; set; } = Guid.NewGuid();
    public GlobalNode Node1 { get; set; } = new();
    public GlobalNode Node2 { get; set; } = new();
    public GlobalNode Node3 { get; set; } = new();
    public GlobalEdge? Edge12 { get; set; }
    public GlobalEdge? Edge13 { get; set; }
    public GlobalEdge? Edge23 { get; set; }
    public GlobalEdge? Edge21 { get; set; }
    public GlobalEdge? Edge31 { get; set; }
    public GlobalEdge? Edge32 { get; set; }
    private const double Ratio = 0.1;

    public bool IsDriectionReverse(TStructureDirection direction, double changeOrientationAngle)
    {
        GlobalNode OriginNode = new();
        GlobalNode ToWardNode1 = new();
        GlobalNode ToWardNode2 = new();
        GlobalEdge ToWardEdge1 = new();
        GlobalEdge ToWardEdge2 = new();

        switch (direction)
        {
            case TStructureDirection.NODE3_NODE2_NODE1:
                if (Edge21 is null || Edge32 is null) return false;
                OriginNode = Node2;
                ToWardNode1 = Node1;
                ToWardNode2 = Node3;
                ToWardEdge1 = Edge21;
                ToWardEdge2 = Edge32;
                break;
            case TStructureDirection.NODE1_NODE2_NODE3:
                if (Edge12 is null || Edge23 is null) return false;
                OriginNode = Node2;
                ToWardNode1 = Node1;
                ToWardNode2 = Node3;
                ToWardEdge1 = Edge12;
                ToWardEdge2 = Edge23;
                break;
            case TStructureDirection.NODE2_NODE3_NODE1:
                if (Edge31 is null || Edge23 is null) return false;
                OriginNode = Node3;
                ToWardNode1 = Node1;
                ToWardNode2 = Node2;
                ToWardEdge1 = Edge23;
                ToWardEdge2 = Edge31;
                break;
            case TStructureDirection.NODE1_NODE3_NODE2:
                if (Edge13 is null || Edge32 is null) return false;
                OriginNode = Node3;
                ToWardNode1 = Node1;
                ToWardNode2 = Node2;
                ToWardEdge1 = Edge13;
                ToWardEdge2 = Edge32;
                break;
            case TStructureDirection.NODE3_NODE1_NODE2:
                if (Edge31 is null || Edge12 is null) return false;
                OriginNode = Node1;
                ToWardNode1 = Node2;
                ToWardNode2 = Node3;
                ToWardEdge1 = Edge31;
                ToWardEdge2 = Edge12;
                break;
            case TStructureDirection.NODE2_NODE1_NODE3:
                if (Edge21 is null || Edge13 is null) return false;
                OriginNode = Node1;
                ToWardNode1 = Node2;
                ToWardNode2 = Node3;
                ToWardEdge1 = Edge21;
                ToWardEdge2 = Edge13;
                break;
        }

        var NearToWardNode1 = MathExtensions.BezierPoint(Ratio, OriginNode, ToWardNode1, ToWardEdge1);
        var NearToWardNode3 = MathExtensions.BezierPoint(Ratio, OriginNode, ToWardNode2, ToWardEdge2);
        var angle = MathExtensions.GetAngle(OriginNode, NearToWardNode1, NearToWardNode3);
        if (angle < changeOrientationAngle) return true;
        return false;
    }

    public bool IsDriectionReverse(GlobalNode node1, GlobalNode node2, double changeOrientationAngle)
    {
        if (node1.Id == Node1.Id)
        {
            if (node2.Id == Node2.Id) return IsDriectionReverse(TStructureDirection.NODE1_NODE3_NODE2, changeOrientationAngle);
            else if (node2.Id == Node3.Id) return IsDriectionReverse(TStructureDirection.NODE1_NODE2_NODE3, changeOrientationAngle);
        }
        else if (node1.Id == Node2.Id)
        {
            if (node2.Id == Node1.Id) return IsDriectionReverse(TStructureDirection.NODE2_NODE3_NODE1, changeOrientationAngle);
            else if (node2.Id == Node3.Id) return IsDriectionReverse(TStructureDirection.NODE2_NODE1_NODE3, changeOrientationAngle);
        }
        else if (node1.Id == Node3.Id)
        {
            if (node2.Id == Node1.Id) return IsDriectionReverse(TStructureDirection.NODE3_NODE2_NODE1, changeOrientationAngle);
            else if (node2.Id == Node2.Id) return IsDriectionReverse(TStructureDirection.NODE3_NODE1_NODE2, changeOrientationAngle);
        }
        return false;
    }

    public GlobalNode? GetIntraNode(GlobalNode node1, GlobalNode node2)
    {
        if (node1.Id == Node1.Id)
        {
            if (node2.Id == Node2.Id) return Node3;
            else if (node2.Id == Node3.Id) return Node2;
        }
        else if (node1.Id == Node2.Id)
        {
            if (node2.Id == Node1.Id) return Node3;
            else if (node2.Id == Node3.Id) return Node1;
        }
        else if (node1.Id == Node3.Id)
        {
            if (node2.Id == Node1.Id) return Node2;
            else if (node2.Id == Node2.Id) return Node1;
        }
        return null;
    }

    public GlobalEdge? GetEdge(GlobalNode node1, GlobalNode node2)
    {
        if (Edge12 is not null && Edge12.StartNodeId == node1.Id && Edge12.EndNodeId == node2.Id) return Edge12;
        if (Edge21 is not null && Edge21.StartNodeId == node1.Id && Edge21.EndNodeId == node2.Id) return Edge21;
        if (Edge13 is not null && Edge13.StartNodeId == node1.Id && Edge13.EndNodeId == node2.Id) return Edge13;
        if (Edge31 is not null && Edge31.StartNodeId == node1.Id && Edge31.EndNodeId == node2.Id) return Edge31;
        if (Edge23 is not null && Edge23.StartNodeId == node1.Id && Edge23.EndNodeId == node2.Id) return Edge23;
        if (Edge32 is not null && Edge32.StartNodeId == node1.Id && Edge32.EndNodeId == node2.Id) return Edge32;
        return null;
    }

    public bool IsAccessDirection(GlobalNode startNode, GlobalNode endNode)
    {
        return GetEdge(startNode, endNode) is not null;
    }
}