RobotNet/RobotNet.RobotManager/Services/Planner/Fokrlift/TStructure.cs

using RobotNet.MapShares;
using RobotNet.MapShares.Dtos;
using RobotNet.MapShares.Enums;

namespace RobotNet.RobotManager.Services.Planner.Fokrlift;
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 NodeDto Node1 { get; set; } = new();
    public NodeDto Node2 { get; set; } = new();
    public NodeDto Node3 { get; set; } = new();
    public EdgeDto Edge12 { get; set; } = new();
    public EdgeDto Edge13 { get; set; } = new();
    public EdgeDto Edge23 { get; set; } = new();
    private const double Ratio = 0.1;

    public bool IsDriectionReverse(TStructureDirection direction)
    {
        NodeDto OriginNode = new();
        NodeDto ToWardNode1 = new();
        NodeDto ToWardNode2 = new();
        EdgeDto ToWardEdge1 = new();
        EdgeDto ToWardEdge2 = new();

        switch (direction)
        {
            case TStructureDirection.NODE3_NODE2_NODE1:
            case TStructureDirection.NODE1_NODE2_NODE3:
                OriginNode = Node2;
                ToWardNode1 = Node1;
                ToWardNode2 = Node3;
                ToWardEdge1 = Edge12;
                ToWardEdge2 = Edge23;
                break;
            case TStructureDirection.NODE2_NODE3_NODE1:
            case TStructureDirection.NODE1_NODE3_NODE2:
                OriginNode = Node3;
                ToWardNode1 = Node1;
                ToWardNode2 = Node2;
                ToWardEdge1 = Edge13;
                ToWardEdge2 = Edge23;
                break;
            case TStructureDirection.NODE3_NODE1_NODE2:
            case TStructureDirection.NODE2_NODE1_NODE3:
                OriginNode = Node1;
                ToWardNode1 = Node2;
                ToWardNode2 = Node3;
                ToWardEdge1 = Edge12;
                ToWardEdge2 = Edge13;
                break;
        }

        var NearToWardNode1 = MapEditorHelper.GetNearByNode(OriginNode, ToWardNode1, ToWardEdge1, Ratio);
        var NearToWardNode3 = MapEditorHelper.GetNearByNode(OriginNode, ToWardNode2, ToWardEdge2, Ratio);
        var angle = MapEditorHelper.GetAngle(OriginNode, NearToWardNode1, NearToWardNode3);
        if (angle < 50) return true;
        return false;
    }

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

    public NodeDto? GetIntraNode(NodeDto node1, NodeDto 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 EdgeDto? GetEdge(NodeDto node1, NodeDto node2)
    {
        if (Edge12.StartNodeId == node1.Id || Edge12.EndNodeId == node1.Id)
        {
            if (Edge12.StartNodeId == node2.Id || Edge12.EndNodeId == node2.Id) return Edge12;
        }
        if (Edge13.StartNodeId == node1.Id || Edge13.EndNodeId == node1.Id)
        {
            if (Edge13.StartNodeId == node2.Id || Edge13.EndNodeId == node2.Id) return Edge13;
        }
        if (Edge23.StartNodeId == node1.Id || Edge23.EndNodeId == node1.Id)
        {
            if (Edge23.StartNodeId == node2.Id || Edge23.EndNodeId == node2.Id) return Edge23;
        }
        return null;
    }

    public bool IsAccessDirection(NodeDto startNode, NodeDto endNode)
    {
        var edge = GetEdge(startNode, endNode);
        if (edge is null) return false;
        if (edge.StartNodeId == startNode.Id && (edge.DirectionAllowed == DirectionAllowed.Both || edge.DirectionAllowed == DirectionAllowed.Forward)) return true;
        if (edge.EndNodeId == startNode.Id && (edge.DirectionAllowed == DirectionAllowed.Both || edge.DirectionAllowed == DirectionAllowed.Backward)) return true;
        return false;
    }
}