using RobotNet.VDA5050.Order;
using RobotNet10.RobotApp.Detection;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Services.ConfigManager;
using RobotNet10.RobotApp.Services.Exceptions;
using RobotNet10.RobotApp.Services.Simulation;
using RobotNet10.RobotApp.Shared.Enums;

namespace RobotNet10.RobotApp.Services.Robot.Modules;

public class RobotNavigation(
    IRobotConfiguration robotConfiguration,
    IServiceProvider serviceProvider,
    RobotNet10.RobotApp.Navigation.NavigationIntegrationService navigationIntegrationService,
    ILogger<RobotNavigation> logger) : INavigation
{
    public bool IsReady { get; private set; }
    private bool _navResultSubscribed;

    public bool Driving
    {
        get
        {
            if (IsSimulation)
                return SimNavigation?.Driving ?? false;

            var feedback = navigationIntegrationService.GetFeedback();
            if (feedback == null) return false;
            return feedback.NavigationState is RobotNet10.RobotApp.Navigation.NavigationState.Controlling;
            // return feedback.NavigationState is RobotNet10.RobotApp.Navigation.NavigationState.Active
            //     or RobotNet10.RobotApp.Navigation.NavigationState.Planning
            //     or RobotNet10.RobotApp.Navigation.NavigationState.Controlling;
        }
    }

    public double VelocityX => IsSimulation ? (SimNavigation?.VelocityX ?? 0) : (navigationIntegrationService.GetTwist()?.x ?? 0);
    public double VelocityY => IsSimulation ? (SimNavigation?.VelocityY ?? 0) : (navigationIntegrationService.GetTwist()?.y ?? 0);
    public double Omega => IsSimulation ? (SimNavigation?.Omega ?? 0) : (navigationIntegrationService.GetTwist()?.theta ?? 0);
    public RobotNet10.RobotApp.Interfaces.NavigationState State => _lastFinishedState ?? (IsSimulation ? (SimNavigation?.State ?? RobotNet10.RobotApp.Interfaces.NavigationState.Idle) : MapNavigationState(navigationIntegrationService.GetFeedback()?.NavigationState));

    public IReadOnlyList<NavigationNode>? CurrentPath
    {
        get
        {
            if (IsSimulation)
                return null;

            var globalPath = navigationIntegrationService.GetGlobalPathData();
            if (globalPath == null || globalPath.Points.Count == 0)
                return null;

            return globalPath.Points.Select(p => new NavigationNode
            {
                Id = Guid.NewGuid(),
                X = p.X,
                Y = p.Y,
                Theta = p.Theta
            }).ToList();
        }
    }

    // C API navigation currently does not expose these dock monitoring values.
    public bool IsDockingActive => false;
    public NavigationNode? DockGoal => null;
    public string DockPhase => string.Empty;
    public string DockDirection => string.Empty;
    public int DockRetryCount => 0;
    public int DockMaxRetries => 0;
    public int DockWaypointCount => 0;
    public NavigationNode? DockStartNode => null;
    public IReadOnlyList<NavigationNode>? DockWaypoints => null;

    private volatile SimulationNavigation? SimNavigation;
    private RobotNet10.RobotApp.Interfaces.NavigationState? _lastFinishedState;
    private bool IsSimulation => robotConfiguration.GetSimulationConfig().IsEnable;

    public event Action<RobotNet10.RobotApp.Interfaces.NavigationState>? OnNavigationFinished;

    public void CancelMovement()
    {
        if (IsSimulation)
        {
            SimNavigation?.CancelMovement();
            return;
        }

        navigationIntegrationService.Cancel();
    }

    public void Move(OrderMsg order, bool hasLoad = false)
    {
        _lastFinishedState = null;
        var nodes = order.Nodes;
        var edges = order.Edges;

        if (IsSimulation)
        {
            if (SimNavigation is not null) throw new NavigationException("The Sim Navigation module is called during operation.");
            SimNavigation = SimulationNavigationManager.GetNavigation(robotConfiguration.GetRobotPhysicalConfig().NavigationType, serviceProvider);
            SimNavigation.OnNavigationFinished += NavigationFinished;
            SimNavigation.Move(order, hasLoad);
            return;
        }

        if (nodes.Length == 0)
            throw new NavigationException("Move failed: nodes list is empty.");

        var target = nodes[^1];
        var (targetX, targetY, theta) = GetNodePose(target, nodes);
        var (qz, qw) = ToYawQuaternion(theta);

        // Convert VDA5050 order (from MQTT server) to OrderData and run full graph navigation
        var orderData = RobotNet10.RobotApp.Navigation.VDA5050ToOrderDataConverter.ToOrderData(nodes, edges, orderMsg: order);
        if (!navigationIntegrationService.MoveToOrder(orderData, targetX, targetY, 0.0, 0.0, 0.0, qz, qw))
            throw new NavigationException("Move failed: Navigation C API service is not ready or rejected goal.");
    }

    public void MoveStraight(double x, double y, bool hasLoad = false, RobotDirection? direction = null)
    {
        if (IsSimulation)
        {
            if (SimNavigation is not null) throw new NavigationException("The Sim Navigation module is called during operation.");
            SimNavigation = SimulationNavigationManager.GetNavigation(robotConfiguration.GetRobotPhysicalConfig().NavigationType, serviceProvider);
            SimNavigation.OnNavigationFinished += NavigationFinished;
            SimNavigation.MoveStraight(x, y, hasLoad, direction);
            return;
        }

        var current = navigationIntegrationService.GetRobotPose2D();
        var currentX = current?.x ?? 0.0;
        var currentY = current?.y ?? 0.0;
        var heading = Math.Atan2(y - currentY, x - currentX);
        var (qz, qw) = ToYawQuaternion(heading);

        if (!navigationIntegrationService.MoveTo(x, y, 0.0, 0.0, 0.0, qz, qw))
            throw new NavigationException("MoveStraight failed: Navigation C API service is not ready or rejected goal.");
    }

    public void Pause()
    {
        if (IsSimulation) SimNavigation?.Pause();
        else navigationIntegrationService.Pause();
    }

    public void Resume()
    {
        if (IsSimulation) SimNavigation?.Resume();
        else navigationIntegrationService.Resume();
    }

    public void Rotate(double angle)
    {
        _lastFinishedState = null;

        if (IsSimulation)
        {
            if (SimNavigation is not null) throw new NavigationException("The Sim Navigation module is called during operation.");
            SimNavigation = SimulationNavigationManager.GetNavigation(robotConfiguration.GetRobotPhysicalConfig().NavigationType, serviceProvider);
            SimNavigation.OnNavigationFinished += NavigationFinished;
            SimNavigation.Rotate(angle * 180 / Math.PI);
            return;
        }

        var pose = navigationIntegrationService.GetRobotPose2D();
        var x = pose?.x ?? 0.0;
        var y = pose?.y ?? 0.0;
        var (qz, qw) = ToYawQuaternion(angle);

        if (!navigationIntegrationService.RotateTo(x, y, 0.0, 0.0, 0.0, qz, qw))
            throw new NavigationException("Rotate failed: Navigation C API service is not ready or rejected goal.");
    }

    public void DockTo(IDetectSession session, bool hasLoad = false, RobotDirection? direction = null)
    {
        _lastFinishedState = null;

        if (IsSimulation)
        {
            if (SimNavigation is not null) throw new NavigationException("The Sim Navigation module is called during operation.");
            SimNavigation = SimulationNavigationManager.GetNavigation(robotConfiguration.GetRobotPhysicalConfig().NavigationType, serviceProvider);
            SimNavigation.OnNavigationFinished += NavigationFinished;
            SimNavigation.DockTo(session, hasLoad, direction);
            return;
        }

        var goal = session.Goal ?? throw new NavigationException("DockTo failed: session goal is missing.");
        var markerName = "dock-marker";
        var p = goal.Pose.Position;
        var o = goal.Pose.Orientation;

        if (!navigationIntegrationService.DockTo(markerName, p.X, p.Y, p.Z, o.X, o.Y, o.Z, o.W))
            throw new NavigationException("DockTo failed: Navigation C API service is not ready or rejected goal.");
    }

    public void RefreshOrder(Node[] nodes, Edge[] edges)
    {
        logger.LogWarning("RefreshOrder is not yet implemented for C API navigation path.");
    }

    public void UpdateOrder(string lastBaseNodeId)
    {
        if (IsSimulation)
        {
            SimNavigation?.UpdateOrder(lastBaseNodeId);
            return;
        }

        logger.LogDebug("UpdateOrder called in C API mode with lastBaseNodeId={LastBaseNodeId}.", lastBaseNodeId);
    }

    public void SafetyStop()
    {
        if (IsSimulation) SimNavigation?.SafetyStop();
        else navigationIntegrationService.Cancel();
    }

    public void Refresh()
    {
        if (IsSimulation) SimNavigation?.Refresh();
    }

    private void NavigationFinished(RobotNet10.RobotApp.Interfaces.NavigationState state)
    {
        _lastFinishedState = state;
        OnNavigationFinished?.Invoke(state);

        if (IsSimulation) SimNavigation?.OnNavigationFinished -= NavigationFinished;
        SimNavigation = null;
    }

    public void SetSpeed(double speed)
    {
        if (IsSimulation) SimNavigation?.SetSpeed(speed);
        else 
        {
            logger.LogInformation("SetSpeed called with speed={Speed}", speed);
            if (!navigationIntegrationService.SetTwistLinear(speed, 0.0, 0.0))
                throw new NavigationException($"SetSpeed failed: unable to set linear velocity to {speed} via Navigation C API.");
        }
    }

    public void Start()
    {
        IsReady = IsSimulation || navigationIntegrationService.IsInitialized;
        if (!IsSimulation && !_navResultSubscribed)
        {
            navigationIntegrationService.OnNavigationResult += OnNavigationResultReceived;
            _navResultSubscribed = true;
        }
    }

    private void OnNavigationResultReceived(RobotNet10.RobotApp.Navigation.NavigationState state)
    {
        var mapped = MapNavigationState(state);
        NavigationFinished(mapped);
    }

    public void Stop()
    {
        if (SimNavigation is not null)
        {
            SimNavigation.CancelMovement();
        }
        else
        {
            navigationIntegrationService.Cancel();
        }
    }

    private static (double qz, double qw) ToYawQuaternion(double yaw)
    {
        var half = yaw / 2.0;
        return (Math.Sin(half), Math.Cos(half));
    }

    private static (double x, double y, double theta) GetNodePose(Node target, Node[] allNodes)
    {
        var (x, y, thetaOpt) = ExtractNodePosition(target);
        if (thetaOpt.HasValue)
            return (x, y, thetaOpt.Value);

        if (allNodes.Length >= 2)
        {
            var (prevX, prevY, _) = ExtractNodePosition(allNodes[^2]);
            return (x, y, Math.Atan2(y - prevY, x - prevX));
        }

        return (x, y, 0.0);
    }

    private static (double x, double y, double? theta) ExtractNodePosition(Node node)
    {
        // VDA5050 Node may store coordinates in NodePosition, while legacy models may use X/Y/Theta directly.
        var nodeType = node.GetType();

        var nodePosProp = nodeType.GetProperty("NodePosition");
        if (nodePosProp?.GetValue(node) is object nodePos)
        {
            var posType = nodePos.GetType();
            var xObj = posType.GetProperty("X")?.GetValue(nodePos);
            var yObj = posType.GetProperty("Y")?.GetValue(nodePos);
            var thetaObj = posType.GetProperty("Theta")?.GetValue(nodePos);

            return (
                xObj is null ? 0.0 : Convert.ToDouble(xObj),
                yObj is null ? 0.0 : Convert.ToDouble(yObj),
                thetaObj is null ? null : Convert.ToDouble(thetaObj));
        }

        var xLegacy = nodeType.GetProperty("X")?.GetValue(node);
        var yLegacy = nodeType.GetProperty("Y")?.GetValue(node);
        var thetaLegacy = nodeType.GetProperty("Theta")?.GetValue(node);

        return (
            xLegacy is null ? 0.0 : Convert.ToDouble(xLegacy),
            yLegacy is null ? 0.0 : Convert.ToDouble(yLegacy),
            thetaLegacy is null ? null : Convert.ToDouble(thetaLegacy));
    }

    private static RobotNet10.RobotApp.Interfaces.NavigationState MapNavigationState(RobotNet10.RobotApp.Navigation.NavigationState? state)
    {
        return state switch
        {
            RobotNet10.RobotApp.Navigation.NavigationState.Pending => RobotNet10.RobotApp.Interfaces.NavigationState.Waiting,
            RobotNet10.RobotApp.Navigation.NavigationState.Planning => RobotNet10.RobotApp.Interfaces.NavigationState.Initializing,
            RobotNet10.RobotApp.Navigation.NavigationState.Active => RobotNet10.RobotApp.Interfaces.NavigationState.Moving,
            RobotNet10.RobotApp.Navigation.NavigationState.Controlling => RobotNet10.RobotApp.Interfaces.NavigationState.Moving,
            RobotNet10.RobotApp.Navigation.NavigationState.Clearing => RobotNet10.RobotApp.Interfaces.NavigationState.Moving,
            RobotNet10.RobotApp.Navigation.NavigationState.Succeeded => RobotNet10.RobotApp.Interfaces.NavigationState.Completed,
            RobotNet10.RobotApp.Navigation.NavigationState.Paused => RobotNet10.RobotApp.Interfaces.NavigationState.Paused,
            RobotNet10.RobotApp.Navigation.NavigationState.Preempted => RobotNet10.RobotApp.Interfaces.NavigationState.Canceled,
            RobotNet10.RobotApp.Navigation.NavigationState.Recalled => RobotNet10.RobotApp.Interfaces.NavigationState.Canceled,
            RobotNet10.RobotApp.Navigation.NavigationState.Rejected => RobotNet10.RobotApp.Interfaces.NavigationState.Error,
            RobotNet10.RobotApp.Navigation.NavigationState.Aborted => RobotNet10.RobotApp.Interfaces.NavigationState.Error,
            RobotNet10.RobotApp.Navigation.NavigationState.Lost => RobotNet10.RobotApp.Interfaces.NavigationState.Error,
            _ => RobotNet10.RobotApp.Interfaces.NavigationState.Idle
        };
    }
}