using RobotNet10.CANOpen.CiA402.Enums;

namespace RobotNet10.CANOpen.CiA402.Models;

public readonly struct Statusword(ushort value)
{
    public ushort Value { get; init; } = value;

    public bool ReadyToSwitchOn => (Value & 0x0001) != 0;
    public bool SwitchedOn => (Value & 0x0002) != 0;
    public bool OperationEnabled => (Value & 0x0004) != 0;
    public bool Fault => (Value & 0x0008) != 0;
    public bool VoltageEnabled => (Value & 0x0010) != 0;
    public bool QuickStop => (Value & 0x0020) != 0;
    public bool SwitchOnDisabled => (Value & 0x0040) != 0;
    public bool Warning => (Value & 0x0080) != 0;
    public bool TargetReached => (Value & 0x0400) != 0;
    
    /// <summary>
    /// CiA402 Statusword bit 12: Homing attained (homing completed successfully)
    /// </summary>
    public bool HomingAttained => (Value & 0x1000) != 0;
    
    /// <summary>
    /// CiA402 Statusword bit 13: Homing error (homing failed)
    /// </summary>
    public bool HomingError => (Value & 0x2000) != 0;
    
    public DriveState GetState()
    {
        if (!ReadyToSwitchOn && !SwitchedOn && !OperationEnabled && !Fault && SwitchOnDisabled)
            return DriveState.SwitchOnDisabled;
        
        if (!ReadyToSwitchOn && !SwitchedOn && !OperationEnabled && !Fault && !SwitchOnDisabled)
            return DriveState.NotReadyToSwitchOn;
        
        if (ReadyToSwitchOn && !SwitchedOn && !OperationEnabled && !Fault)
            return DriveState.ReadyToSwitchOn;
        
        if (ReadyToSwitchOn && SwitchedOn && !OperationEnabled && !Fault)
            return DriveState.SwitchedOn;
        
        if (ReadyToSwitchOn && SwitchedOn && OperationEnabled && !Fault)
            return DriveState.OperationEnabled;
        
        if (!ReadyToSwitchOn && !SwitchedOn && !OperationEnabled && Fault)
            return DriveState.Fault;
        
        return DriveState.Unknown;
    }
    
    public override string ToString() => $"0x{Value:X4} ({GetState()})";
    
    public static implicit operator ushort(Statusword sw) => sw.Value;
    public static implicit operator Statusword(ushort value) => new(value);
}