@using RobotNet10.RobotApp.Client.Shared.Motion
@using RobotNet10.Shared.Geometry
@using RobotNet10.Shared.Numbers
@using QuaternionGeometry = RobotNet10.Shared.Geometry.Quaternion
@using QuaternionNumbers = RobotNet10.Shared.Numbers.Quaternion
@using MudBlazor
@implements IDisposable

<MudPaper Class="pa-4" Style="height: 100%;">
    <MudStack Spacing="3">
        <MudStack Row="true" Justify="Justify.SpaceBetween" AlignItems="AlignItems.Center">
            <MudText Typo="Typo.h5">Odometry</MudText>
        </MudStack>

        @* Odometry Information *@
        @if (Odometry != null)
        {
            <MudCard>
                <MudCardContent>
                    <MudText Typo="Typo.h6" Class="mb-3">Position</MudText>
                    <MudStack Spacing="2">
                        <MudStack Row="true" Justify="Justify.SpaceBetween">
                            <MudText>X:</MudText>
                            <MudText>@($"{Odometry.PositionX:F3} m")</MudText>
                        </MudStack>
                        <MudStack Row="true" Justify="Justify.SpaceBetween">
                            <MudText>Y:</MudText>
                            <MudText>@($"{Odometry.PositionY:F3} m")</MudText>
                        </MudStack>
                        <MudStack Row="true" Justify="Justify.SpaceBetween">
                            <MudText>Z:</MudText>
                            <MudText>@($"{Odometry.PositionZ:F3} m")</MudText>
                        </MudStack>
                    </MudStack>
                </MudCardContent>
            </MudCard>

            <MudCard>
                <MudCardContent>
                    <MudText Typo="Typo.h6" Class="mb-3">Orientation</MudText>
                    <MudStack Spacing="2">
                        <MudStack Row="true" Justify="Justify.SpaceBetween">
                            <MudText>Yaw:</MudText>
                            <MudText>@($"{yawDegrees:F2}°")</MudText>
                        </MudStack>
                        <MudStack Row="true" Justify="Justify.SpaceBetween">
                            <MudText>Update Freq:</MudText>
                            <MudChip T="double" Color="@(Odometry.UpdateFrequency > 0 ? Color.Success : Color.Default)" Size="Size.Small">
                                @($"{Odometry.UpdateFrequency:F2} Hz")
                            </MudChip>
                        </MudStack>
                    </MudStack>
                </MudCardContent>
            </MudCard>
        }
        else
        {
            <MudAlert Severity="Severity.Info">No odometry data available.</MudAlert>
        }
    </MudStack>
</MudPaper>

@code {
    [Parameter, EditorRequired] public OdometryDto? Odometry { get; set; }
    [Parameter] public bool IsHubReady { get; set; }
    [Inject] private ISnackbar Snackbar { get; set; } = null!;

    private double _smoothedYawDegrees;
    private bool _hasYaw;

    private double yawDegrees => _smoothedYawDegrees;

    protected override void OnParametersSet()
    {
        if (Odometry == null)
            return;

        // Nếu robot gần như đứng yên (ít quay, ít chạy thẳng) thì giữ nguyên yaw
        var angularZ = Odometry.AngularVelocityZ;   // rad/s
        var linearX = Odometry.LinearVelocityX;     // m/s
        const double angularDeadZone = 0.01;        // ~0.57°
        const double linearDeadZone = 0.005;        // 5 mm/s

        var isAlmostStopped =
            Math.Abs(angularZ) < angularDeadZone &&
            Math.Abs(linearX) < linearDeadZone;

        // Lần đầu vẫn phải init giá trị
        if (!_hasYaw)
        {
            var q0 = new QuaternionGeometry(Odometry.OrientationX, Odometry.OrientationY, Odometry.OrientationZ, Odometry.OrientationW);
            _smoothedYawDegrees = q0.ToYawDegrees();
            _hasYaw = true;
            return;
        }

        if (isAlmostStopped)
        {
            // Robot đứng yên: không cập nhật yaw để tránh drift chậm
            return;
        }

        // Robot đang quay / di chuyển: cho phép yaw thay đổi nhưng có lọc
        var quaternion = new QuaternionGeometry(Odometry.OrientationX, Odometry.OrientationY, Odometry.OrientationZ, Odometry.OrientationW);
        var newYaw = quaternion.ToYawDegrees();

        // Normalize delta để tránh nhảy 360° khi wrap-around
        var delta = newYaw - _smoothedYawDegrees;
        while (delta > 180.0) delta -= 360.0;
        while (delta < -180.0) delta += 360.0;

        // Low-pass filter để làm mượt (alpha càng nhỏ càng mượt)
        const double alpha = 0.3;
        _smoothedYawDegrees = _smoothedYawDegrees + alpha * delta;
    }

    public void Dispose()
    {
        // Nothing to dispose currently
    }
}