BQP/srcs/RobotNet10/RobotApp/Communication/Sick.SafetyScanners/DataStructures/UdpScanData.cs

namespace Sick.SafetyScanners.DataStructures;

/// <summary>
/// Contains all data blocks from a UDP scan data packet
/// </summary>
public sealed class UdpScanData
{
    /// <summary>
    /// Gets or sets the data header
    /// </summary>
    public DataHeader Header { get; init; } = new DataHeader();
    
    /// <summary>
    /// Gets or sets the general system state
    /// </summary>
    public GeneralSystemState? GeneralSystemState { get; init; }
    
    /// <summary>
    /// Gets or sets the derived values (configuration of data output)
    /// </summary>
    public DerivedValues? DerivedValues { get; init; }
    
    /// <summary>
    /// Gets or sets the measurement data (scan points)
    /// </summary>
    public MeasurementData? MeasurementData { get; init; }
    
    /// <summary>
    /// Gets or sets the intrusion data (field interruption)
    /// </summary>
    public IntrusionData? IntrusionData { get; init; }
    
    /// <summary>
    /// Gets or sets the application data (inputs and outputs)
    /// </summary>
    public ApplicationData? ApplicationData { get; init; }
    
    /// <summary>
    /// Gets the timestamp from the header as DateTime (if available)
    /// Returns UTC DateTime to ensure consistency with Cartographer timestamp handling
    /// 
    /// SICK Timestamp Format (according to official documentation):
    /// - TimestampDate (ushort, offset 24): 
    ///   * If UTC server is time master: Days since 1972-01-01
    ///   * If Safety Designer device is time master: Number of full 24-hour cycles of time master
    ///   * If no time sync: Number of full 24-hour cycles since device was switched on
    /// - TimestampTime (uint, offset 28): Milliseconds since midnight (or start of 24-hour cycle)
    /// 
    /// Since we cannot detect time sync status from UDP packet, we try both base dates:
    /// 1. Try 1972-01-01 (UTC server case) - most common in production
    /// 2. If result is unreasonable, try 1980-01-01 (legacy/fallback)
    /// 3. If TimestampDate = 0: Use current date as base
    /// </summary>
    public DateTime? Timestamp
    {
        get
        {
            if (Header.TimestampDate == 0 && Header.TimestampTime == 0)
                return null;
            
            DateTime baseDate;
            string baseDateSource = "unknown";
            
            if (Header.TimestampDate == 0)
            {
                // Date not set on device - use current date as base
                var now = DateTime.UtcNow;
                baseDate = new DateTime(now.Year, now.Month, now.Day, 0, 0, 0, DateTimeKind.Utc);
                baseDateSource = "current_date";
            }
            else
            {
                // Try 1972-01-01 first (UTC server case - most common according to documentation)
                var baseDate1972 = new DateTime(1972, 1, 1, 0, 0, 0, DateTimeKind.Utc);
                var time1972 = baseDate1972.AddDays(Header.TimestampDate).AddMilliseconds(Header.TimestampTime);
                
                // Check if timestamp with 1972 base is reasonable (within 1 day of now)
                var currentTimeTicks = DateTime.UtcNow.Ticks;
                var time1972Ticks = time1972.Ticks;
                var diff1972Ms = Math.Abs(time1972Ticks - currentTimeTicks) / TimeSpan.TicksPerMillisecond;
                
                if (diff1972Ms <= 86400000) // Within 1 day - reasonable
                {
                    baseDate = baseDate1972;
                    baseDateSource = "1972-01-01 (UTC server)";
                }
                else
                {
                    // Try 1980-01-01 as fallback (legacy format or no UTC sync)
                    var baseDate1980 = new DateTime(1980, 1, 1, 0, 0, 0, DateTimeKind.Utc);
                    var time1980 = baseDate1980.AddDays(Header.TimestampDate).AddMilliseconds(Header.TimestampTime);
                    var time1980Ticks = time1980.Ticks;
                    var diff1980Ms = Math.Abs(time1980Ticks - currentTimeTicks) / TimeSpan.TicksPerMillisecond;
                    
                    if (diff1980Ms <= diff1972Ms) // 1980 is closer to now
                    {
                        baseDate = baseDate1980;
                        baseDateSource = "1980-01-01 (legacy/fallback)";
                    }
                    else
                    {
                        // 1972 is closer, use it even if not perfect
                        baseDate = baseDate1972;
                        baseDateSource = "1972-01-01 (best match)";
                    }
                }
            }
            
            // Add days and milliseconds to base date
            var time = baseDate.AddDays(Header.TimestampDate).AddMilliseconds(Header.TimestampTime);
            
            // Validate: timestamp should not be too far in the past or future
            var nowTicks = DateTime.UtcNow.Ticks;
            var timeTicks = time.Ticks;
            var diffMs = Math.Abs(timeTicks - nowTicks) / TimeSpan.TicksPerMillisecond;
            
            // Debug logging for timestamp calculation
            if (diffMs > 3600000) // Log if more than 1 hour off
            {
                var timestampDateStr = Header.TimestampDate == 0 ? "0 (using current date)" : Header.TimestampDate.ToString();
                var timestampTimeStr = $"{Header.TimestampTime}ms ({Header.TimestampTime / 3600000.0:F2} hours)";
                var calculatedTime = time.ToString("yyyy-MM-dd HH:mm:ss.fff");
                System.Diagnostics.Debug.WriteLine(
                    $"[SICK Timestamp Debug] TimestampDate={timestampDateStr}, TimestampTime={timestampTimeStr}, " +
                    $"BaseDateSource={baseDateSource}, CalculatedTime={calculatedTime}, " +
                    $"DiffFromNow={diffMs / 3600000.0:F2} hours");
            }
            
            // If timestamp is more than 1 day off, it's likely incorrect
            // This can happen if device date is not set correctly or wrong base date
            if (diffMs > 86400000) // 1 day in milliseconds
            {
                // Use current time instead of potentially incorrect device timestamp
                return DateTime.UtcNow;
            }
            
            // Ensure the result is marked as UTC
            return DateTime.SpecifyKind(time, DateTimeKind.Utc);
        }
    }
}