using RobotNet.VDA5050.Factsheet;
using RobotNet.VDA5050.Type;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Services.Exceptions;
using System.Collections.ObjectModel;

namespace RobotNet10.RobotApp.Services.Robot.Actions;

[RobotAction(ActionType.INIT_POSITION,
            [ActionScope.INSTANT, ActionScope.NODE],
            [BlockingType.HARD],
            "Khởi tạo lại vị trí robot.",
            "Robot đã khởi tạo lại vị trí.")]
public class RobotInitPositionAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
    private static readonly ReadOnlyCollection<ActionParameter> ActionParameters = new List<ActionParameter>
    {
        new()
        {
            Key = "x",
            Description = "Tọa độ X của vị trí khởi tạo.",
            ValueDataType = ValueDataType.FLOAT,
            IsOptional = false,
        },
        new()
        {
            Key = "y",
            Description = "Tọa độ Y của vị trí khởi tạo.",
            ValueDataType = ValueDataType.FLOAT,
            IsOptional = false,
        },
        new()
        {
            Key = "theta",
            Description = "Góc quay (theta) của vị trí khởi tạo. (rad)",
            ValueDataType = ValueDataType.FLOAT,
            IsOptional = false,
        }
    }.AsReadOnly();

    double X = 0;
    double Y = 0;
    double Theta = 0;
    ILocalization? Localization;

    protected override Task StartAction()
    {
        Localization = ServiceProvider.GetRequiredService<ILocalization>();
        var initPose = Localization.SetInitializePosition(X, Y, Theta);
        if (!initPose.IsSuccess)
        {
            SetStatus(ActionEvent.FAILED);
            ResultDescription = initPose.Message;
        }
        else
        {
            SetStatus(ActionEvent.FINISHED);
            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
        }
        return base.StartAction();
    }

    protected override void Initialize()
    {
        base.Initialize();

        if (ActionParameters.Count > 0)
        {
            foreach (var parameterStore in ActionParameters)
            {
                if (!parameterStore.IsOptional)
                {
                    if (Action is null || Action.ActionParameters is null || !Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}.");
                }
            }
            Parameters = Action is null || Action.ActionParameters is null ? [] : Action.ActionParameters;
        }

        var xPara = Parameters.FirstOrDefault(p => p.Key == "x") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'x'");
        var yPara = Parameters.FirstOrDefault(p => p.Key == "y") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'y'");
        var thetaPara = Parameters.FirstOrDefault(p => p.Key == "theta") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'theta'");

        var xParse = double.TryParse(xPara.Value, out double xData);
        var yParse = double.TryParse(yPara.Value, out double yData);
        var thetaParse = double.TryParse(thetaPara.Value, out double thetaData);

        if (!xParse) throw new ActionException($"Action {Type} có parameter 'x' không đúng kiểu dữ liệu");
        if (!yParse) throw new ActionException($"Action {Type} có parameter 'y' không đúng kiểu dữ liệu");
        if (!thetaParse) throw new ActionException($"Action {Type} có parameter 'theta' không đúng kiểu dữ liệu");
        X = xData;
        Y = yData;
        Theta = thetaData;
    }
}