using RobotNet10.CANOpen.CiA402.Enums;
using RobotNet10.Shared.Geometry;
using System.Threading;
using System.Threading.Tasks;

namespace RobotNet10.RobotApp.Motion;

/// <summary>
/// Interface cho Inverse Kinematics - chuyển đổi vận tốc robot (Twist) thành vận tốc các bánh xe và điều khiển robot
/// Hỗ trợ state machine và operation modes tương tự CiA402
/// </summary>
public interface IInverseKinematics
{
    /// <summary>
    /// Gets the current state of the drive system (tương tự CiA402 state machine)
    /// </summary>
    bool IsOperationEnabled { get; }

    /// <summary>
    /// Tính toán vận tốc các bánh xe từ vận tốc robot
    /// </summary>
    /// <param name="twist">Vận tốc robot (linear X, angular Z)</param>
    /// <returns>Tuple (leftWheelVelocity, rightWheelVelocity) - vận tốc bánh trái và bánh phải (encoder counts/s)</returns>
    (int leftWheelVelocity, int rightWheelVelocity) CalculateWheelVelocities(Twist twist);

    /// <summary>
    /// Đặt vận tốc robot (Twist) - sẽ tự động tính toán và điều khiển các bánh xe
    /// </summary>
    /// <param name="twist">Vận tốc robot (linear X, angular Z)</param>
    /// <param name="ct">Cancellation token</param>
    Task SetVelocityAsync(Twist twist, CancellationToken ct = default);

    /// <summary>
    /// Enable drive system - convenience method để tự động transition qua các states
    /// đến OperationEnabled. Gọi method này sẽ tự động thực hiện tất cả các bước cần thiết.
    /// </summary>
    void Enable();

    /// <summary>
    /// Enable 2 động cơ giống enable bằng tay trên device: gửi lệnh trực tiếp xuống từng servo,
    /// await từng bước (SetOperationMode, SetProfileVelocity 0, SwitchOn, EnableOperation) rồi mới xong.
    /// </summary>
    Task EnableAsync(CancellationToken cancellationToken = default);

    /// <summary>
    /// Disable drive system - convenience method để tắt drive về trạng thái an toàn
    /// </summary>
    void Disable();

    /// <summary>
    /// Enable operation - chuyển từ SwitchedOn sang OperationEnabled (single step)
    /// </summary>
    void EnableOperation();

    /// <summary>
    /// Disable operation - chuyển từ OperationEnabled về SwitchedOn (single step)
    /// </summary>
    void DisableOperation();

    /// <summary>
    /// Quick stop - dừng khẩn cấp drive system
    /// </summary>
    void QuickStop();

    /// <summary>
    /// Fault reset - reset lỗi và đưa drive về trạng thái ban đầu
    /// </summary>
    void FaultReset();

    /// <summary>
    /// Set operation mode cho drive system (ProfileVelocity, ProfilePosition, etc.)
    /// </summary>
    /// <param name="mode">Operation mode to set</param>
    /// <param name="ct">Cancellation token</param>
    Task SetOperationModeAsync(OperationMode mode, CancellationToken ct = default);

    /// <summary>
    /// Get current operation mode của drive system
    /// </summary>
    /// <param name="ct">Cancellation token</param>
    Task<OperationMode> GetOperationModeAsync(CancellationToken ct = default);

    /// <summary>
    /// Set profile acceleration cho drive system
    /// </summary>
    /// <param name="acceleration">Gia tốc tăng tốc (m/s²)</param>
    /// <param name="ct">Cancellation token</param>
    Task SetAccelerationAsync(double acceleration, CancellationToken ct = default);

    /// <summary>
    /// Set profile deceleration cho drive system
    /// </summary>
    /// <param name="deceleration">Gia tốc giảm tốc (m/s²)</param>
    /// <param name="ct">Cancellation token</param>
    Task SetDecelerationAsync(double deceleration, CancellationToken ct = default);
}