APM/Assets/Scripting/Robot/VisualizationSender.cs

using Newtonsoft.Json;
using System;
using System.Linq;
using System.Threading.Tasks;
using UnityEngine;

public class VisualizationSender
{
    private readonly MqttClientManager mqttClientManager;
    private readonly OrderProcessor orderProcessor;
    public readonly Transform transform;
    private readonly string visualizationTopic;
    private readonly string stateTopic;
    public readonly string serialNumber;
    private readonly string currentMapId;
    private readonly PathMover pathMover;
    public Vector3 velocity;
    public float angularVelocity;

    public VisualizationSender(
        MqttClientManager mqttClientManager,
        Transform transform,
        OrderProcessor orderProcessor,
        string visualizationTopic,
        string stateTopic,
        string serialNumber,
        string currentMapId,
        PathMover pathMover)
    {
        this.mqttClientManager = mqttClientManager;
        this.orderProcessor = orderProcessor;
        this.transform = transform;
        this.visualizationTopic = visualizationTopic;
        this.stateTopic = stateTopic;
        this.serialNumber = serialNumber;
        this.currentMapId = currentMapId;
        this.pathMover = pathMover;
        velocity = Vector3.zero;
        angularVelocity = 0f;
        //Debug.Log($"Khởi tạo VisualizationSender cho robot với serialNumber: {serialNumber}");
    }

    public void SetVelocity(Vector3 velocity, float angularVelocity)
    {
        this.velocity = velocity;
        this.angularVelocity = angularVelocity;
    }

    // Hàm chuẩn hóa góc trong khoảng [-π, π]
    public float NormalizeAngle(float angleRad)
    {
        while (angleRad > Mathf.PI) angleRad -= 2 * Mathf.PI;
        while (angleRad < -Mathf.PI) angleRad += 2 * Mathf.PI;
        return angleRad;
    }

    // Hàm chuẩn hóa góc độ trong khoảng [-180°, 180°]
    public float NormalizeEulerAngle(float angleDeg)
    {
        while (angleDeg > 180f) angleDeg -= 360f;
        while (angleDeg < -180f) angleDeg += 360f;
        return angleDeg;
    }

    public async void SendVisualization()
    {
        try
        {
            if (transform == null)
            {
                Debug.LogError("Transform is null in SendVisualization.");
                return;
            }

            Vector3 position = transform.position;
            float theta = NormalizeAngle(2 * Mathf.PI - transform.eulerAngles.y * Mathf.Deg2Rad); // Công thức mới
            Vector3 vel = velocity;
            float omega = angularVelocity;
            float displayEulerY = NormalizeEulerAngle(transform.eulerAngles.y); // Giữ nguyên hiển thị

            string json = await Task.Run(() =>
            {
                var visualizationData = new VisualizationData
                {
                    HeaderId = 1,
                    Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
                    Version = "1.0.0",
                    Manufacturer = "phenikaaX",
                    SerialNumber = serialNumber,
                    MapId = currentMapId,
                    MapDescription = string.IsNullOrEmpty(currentMapId) ? "" : "Default map",
                    AgvPosition = new AgvPosition
                    {
                        X = position.x,
                        Y = position.z,
                        MapId = currentMapId,
                        Theta = theta,
                        PositionInitialized = true,
                        LocalizationScore = 0.7f,
                        DeviationRange = 0.1f
                    },
                    Velocity = new Velocity
                    {
                        Vx = vel.x,
                        Vy = vel.z,
                        Omega = omega
                    }
                };
                return JsonConvert.SerializeObject(visualizationData, Formatting.None);
            });

            await mqttClientManager.PublishAsync(visualizationTopic, json);
        }
        catch (Exception ex)
        {
            Debug.LogError($"Lỗi khi gửi visualization cho robot với serialNumber {serialNumber}: {ex.Message}");
        }
    }

    public async void SendState()
    {
        try
        {
            if (transform == null)
            {
                Debug.LogError("Transform is null in SendState.");
                return;
            }

            Vector3 position = transform.position;
            float theta = NormalizeAngle(2 * Mathf.PI - transform.eulerAngles.y * Mathf.Deg2Rad); // Công thức mới
            Vector3 vel = velocity;
            float omega = angularVelocity;
            float displayEulerY = NormalizeEulerAngle(transform.eulerAngles.y); // Giữ nguyên hiển thị

            string json = await Task.Run(() =>
            {
                var order = orderProcessor.CurrentOrder;
                var nodeStates = order?.Nodes?.Select(n => new NodeState
                {
                    NodeId = n.NodeId,
                    SequenceId = n.SequenceId,
                    NodeDescription = n.NodeDescription ?? "",
                    Released = n.Released,
                    NodePosition = new NodePosition
                    {
                        X = n.NodePosition.X,
                        Y = n.NodePosition.Y,
                        Theta = n.NodePosition.Theta, // Giữ nguyên theta từ JSON
                        AllowedDeviationXY = n.NodePosition.AllowedDeviationXY,
                        AllowedDeviationTheta = n.NodePosition.AllowedDeviationTheta,
                        MapId = n.NodePosition.MapId,
                        MapDescription = n.NodePosition.MapDescription ?? ""
                    }
                }).ToArray() ?? new NodeState[0];

                var edgeStates = order?.Edges?.Select(e => new EdgeState
                {
                    EdgeId = e.EdgeId,
                    SequenceId = e.SequenceId,
                    EdgeDescription = e.EdgeDescription ?? "",
                    Released = e.Released,
                    Trajectory = e.Trajectory != null ? new Trajectory
                    {
                        Degree = e.Trajectory.Degree,
                        KnotVector = e.Trajectory.KnotVector,
                        ControlPoints = e.Trajectory.ControlPoints?.Select(cp => new ControlPoint
                        {
                            X = cp.X,
                            Y = cp.Y,
                            Weight = cp.Weight
                        }).ToArray()
                    } : null
                }).ToArray() ?? new EdgeState[0];

                var stateData = new SendState
                {
                    HeaderId = 1,
                    Timestamp = DateTime.UtcNow.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
                    Version = "1.0.0",
                    Manufacturer = "PhenikaaX",
                    SerialNumber = serialNumber,
                    Maps = new Map[]
                    {
                    new()
                    {
                        MapId = currentMapId,
                        MapVersion = "1.0",
                        MapDescription = "Default map",
                        MapStatus = "ENABLED"
                    }
                    },
                    OrderId = orderProcessor.OrderId ?? "",
                    OrderUpdateId = orderProcessor.OrderId != null ? 1 : 0,
                    ZoneSetId = "",
                    LastNodeId = orderProcessor.LastNodeId ?? "",
                    LastNodeSequenceId = orderProcessor.LastNodeSequenceId,
                    NodeStates = nodeStates,
                    EdgeStates = edgeStates,
                    Driving = pathMover.IsMoving,
                    Paused = !pathMover.IsMoving,
                    NewBaseRequest = !pathMover.IsMoving,
                    DistanceSinceLastNode = 1.531483174223427f,
                    AgvPosition = new AgvPosition
                    {
                        X = position.x,
                        Y = position.z,
                        Theta = theta,
                        MapId = currentMapId,
                        MapDescription = "",
                        PositionInitialized = true,
                        LocalizationScore = 0.7f,
                        DeviationRange = 0.1f
                    },
                    Velocity = new Velocity
                    {
                        Vx = vel.x,
                        Vy = vel.z,
                        Omega = omega
                    },
                    Loads = new Load[0],
                    ActionStates = new ActionState[0],
                    BatteryState = new BatteryState
                    {
                        BatteryCharge = 80f,
                        BatteryVoltage = 48f,
                        BatteryHealth = 100f,
                        Charging = false,
                        Reach = 1000f
                    },
                    OperatingMode = "AUTOMATIC",
                    Errors = new Error[0],
                    Information = new Information[0],
                    SafetyState = new SafetyState
                    {
                        EStop = "NONE",
                        FieldViolation = false
                    }
                };
                return JsonConvert.SerializeObject(stateData, Formatting.None);
            });

            await mqttClientManager.PublishAsync(stateTopic, json);
        }
        catch (Exception ex)
        {
            Debug.LogError($"Lỗi khi gửi state cho robot với serialNumber {serialNumber}: {ex.Message}");
        }
    }

}