using RobotNet.VDA5050.Visualization;
using RobotNet10.Common;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Services.ConfigManager;
using RobotNet10.RobotApp.Services.Robot.Connection;

namespace RobotNet10.RobotApp.Services.Robot;

public class RobotVisualization(ILocalization Localization,
                                INavigation Navigation,
                                IConnectionConfig ConnectionConfig,
                                IRobotConnectionsService RobotConnectionsService,
                                ILogger<RobotVisualization> Logger)
{
    private uint HeaderId;

    private WatchThread<RobotVisualization>? UpdateTimer;
    private const int UpdateInterval = 100;
    private VisualizationMsg GetVisualizationMsg()
    {
        var vdaConfig = ConnectionConfig.GetVDA5050Config();
        return new VisualizationMsg()
        {
            HeaderId = HeaderId++,
            Manufacturer = vdaConfig.Manufacturer,
            Version = vdaConfig.Version,
            SerialNumber = vdaConfig.SerialNumber,
            AgvPosition = new AgvPosition()
            {
                X = Localization.X,
                Y = Localization.Y,
                Theta = Localization.Theta
            },
            Velocity = new Velocity()
            {
                Vx = Navigation.VelocityX,
                Vy = Navigation.VelocityY,
                Omega = Navigation.Omega
            }
        };
    }

    private void UpdateHandler()
    {
        try
        {
            if (!RobotConnectionsService.IsConnected) return;
            var publish = RobotConnectionsService.PublishVisualizationAsync(GetVisualizationMsg());
            publish.ConfigureAwait(false);
        }
        catch { }
    }

    public void Start()
    {
        if (UpdateTimer is not null) Stop();

        UpdateTimer = new(UpdateInterval, UpdateHandler, Logger, ThreadPriority.Normal);
        UpdateTimer.Start();
    }

    public void Stop()
    {
        UpdateTimer?.Dispose();
        UpdateTimer = null;
    }
}