using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Xloc;
using RobotNet10.RobotApp.Services.ConfigManager;
using RobotNet10.RobotApp.Services.Simulation;
using RobotNet10.Shared;
using RobotNet10.Shared.Geometry;
using RobotNet10.RobotApp.Shared.Enums;

namespace RobotNet10.RobotApp.Services.Robot.Modules;


public class RobotLocalization(IRobotConfiguration RobotConfiguration,
                               XlocIntegrationService xlocService,
                               SimulationVisualization SimVisualization,
                               Logger<RobotLocalization> Logger) 
                               : ILocalization
{
    public double X => IsSimulation ? SimVisualization.X : GetXlocX();
    public double Y => IsSimulation ? SimVisualization.Y : GetXlocY();
    public double Theta => IsSimulation ? SimVisualization.Theta * Math.PI / 180 : GetXlocTheta();
    public bool IsReady => IsSimulation ? true : IsXlocReady();
    public string CurrentActiveMap => IsSimulation ? "" : GetXlocCurrentActiveMap();
    public double DeviationRange { get; private set; }
    public double LocalizationScore => IsSimulation ? 1.0 : GetXlocLocalizationScore();
    public bool PositionInitialized => IsSimulation ? true : GetXlocPositionInitialized();
    private bool IsSimulation => RobotConfiguration.GetSimulationConfig().IsEnable;

    private double GetXlocX()
    {
        var pose = xlocService.GetCurrentPose2D();
        return pose?.x ?? 0.0;
    }

    private double GetXlocY()
    {
        var pose = xlocService.GetCurrentPose2D();
        return pose?.y ?? 0.0;
    }

    private double GetXlocTheta()
    {
        var pose = xlocService.GetCurrentPose2D();
        return pose?.yaw ?? 0.0;
    }

    private bool IsXlocReady()
    {
        var diagnostics = xlocService.GetDiagnostics();
        // 0=MAPPING, 1=LOCALIZATION, 2=PROCESSING, 3=READY, 4=ERROR. Accept 1,2,3 so orders are allowed once localizing.
        if (diagnostics == null) return false;
        return diagnostics.XlocState is 1 or 2 or 3;
    }

    private string GetXlocCurrentActiveMap()
    {
        var diagnostics = xlocService.GetDiagnostics();
        return diagnostics?.CurrentActiveMap ?? "";
    }

    private double GetXlocLocalizationScore()
    {
        var diagnostics = xlocService.GetDiagnostics();
        return diagnostics?.Reliability ?? 0.0; // Use Reliability (0.0 to 1.0) as LocalizationScore
    }

    private bool GetXlocPositionInitialized()
    {
        // Position is initialized if we have a valid pose from XLOC and it's not in ERROR state
        var pose = xlocService.GetCurrentPose2D();
        var diagnostics = xlocService.GetDiagnostics();
        
        return pose.HasValue && diagnostics?.XlocState != 4; // 4 = ERROR
    }

    public double DistanceTo(double x, double y)
    {
        return Math.Sqrt(Math.Pow(x - X, 2) + Math.Pow(y - Y, 2));
    }

    public MessageResult SetInitializePosition(double x, double y, double theta)
    {
        try
        {
            if (IsSimulation)
            {
                SimVisualization.LocalizationInitialize(x, y, theta * 180 / Math.PI);
                return new(true);
            }
            else
            {
                // Use XlocIntegrationService to set initial pose
                // theta is in radians, convert to radians for xloc (it expects radians)
                bool result = xlocService.SetInitialPose(x, y, 0.0, 0.0, 0.0, theta);
                if (result)
                {
                    return new(true, "Initial position set successfully");
                }
                else
                {
                    return new(false, "Failed to set initial position");
                }
            }
        }
        catch (Exception ex)
        {
            Logger.Warning($"Initialize robot position failed: {ex.Message}");
            return new(false, $"Initialize robot position failed: {ex.Message}");
        }
    }

    // private bool GetIsReady()
    // {
    //     if (IsSimulation) return true;
    //     return xlocService.IsReady;
    // }
}