Denso/srcs/RobotNet10/RobotApp/Communication/Olei.LidarSensor/USAGE_EXAMPLE.cs

using Olei.LidarSensor;

namespace Olei.LidarSensor.Examples;

/// <summary>
/// Example usage of Olei LiDAR Server
/// Demonstrates how to receive and process LiDAR data
/// </summary>
public class UsageExample
{
    public static void BasicExample()
    {
        // Create server on UDP port 2368 (typical LiDAR port)
        using var server = new OleiLidarServer(2368);

        // Subscribe to data received event
        server.DataReceived += OnDataReceived;

        // Subscribe to error event
        server.ErrorOccurred += OnError;

        // Start receiving data
        server.Start();

        Console.WriteLine("LiDAR server started. Press any key to stop...");
        Console.ReadKey();

        // Stop server (also called automatically by Dispose)
        server.Stop();

        // Print statistics
        Console.WriteLine(server.GetStatistics());
    }

    private static void OnDataReceived(object? sender, LidarDataPacket e)
    {
        var packet = e;

        // Check if header is valid
        if (!packet.Header.IsValidFrame)
        {
            Console.WriteLine("Invalid frame received!");
            return;
        }

        // Check for errors
        if (packet.Header.HasError)
        {
            Console.WriteLine($"LiDAR Error - Motor: {packet.Header.HasMotorFault}, " +
                            $"Voltage: {packet.Header.HasAbnormalVoltage}, " +
                            $"Temperature: {packet.Header.HasTemperatureFault}");
        }

        // Print header info
        Console.WriteLine($"Rotation Rate: {packet.Header.RotationRate} rpm, " +
                         $"Direction: {(packet.Header.IsCounterClockwise ? "CCW" : "CW")}, " +
                         $"Valid Points: {packet.GetValidDataBlockCount()}");

        // Process valid data points
        ProcessDataPoints(packet);

        // NOTE: Packet is automatically returned to pool after this handler completes
    }

    private static void ProcessDataPoints(LidarDataPacket packet)
    {
        byte distanceScale = packet.Header.DistanceScale;

        // Method 1: Iterate through all data blocks
        for (int i = 0; i < LidarDataPacket.DATA_BLOCK_COUNT; i++)
        {
            var block = packet.DataBlocks[i];
            if (block.IsValid)
            {
                double angle = block.GetAngleDegrees();
                double distance = block.GetDistance(distanceScale);
                ushort strength = block.SignalStrength;

                // Process data point
                // Console.WriteLine($"Angle: {angle:F2}°, Distance: {distance:F2}, Strength: {strength}");
            }
        }

        // Method 2: Use GetValidDataBlocks (cleaner but with slight overhead)
        foreach (var block in packet.GetValidDataBlocks())
        {
            double angle = block.GetAngleDegrees();
            double distance = block.GetDistance(distanceScale);
            // Process...
        }

        // Method 3: Get as Cartesian coordinates
        foreach (var (x, y) in packet.GetValidCartesianPoints())
        {
            // Use X, Y coordinates directly
            // Console.WriteLine($"X: {x:F2}, Y: {y:F2}");
        }
    }

    private static void OnError(object? sender, LidarErrorEventArgs e)
    {
        Console.WriteLine($"[ERROR] {e.Message}");
        if (e.Exception != null)
        {
            Console.WriteLine($"Exception: {e.Exception}");
        }
    }

    /// <summary>
    /// Advanced example with custom port and statistics monitoring
    /// </summary>
    public static void AdvancedExample()
    {
        const int port = 2368;
        using var server = new OleiLidarServer(port);

        int packetCount = 0;
        var startTime = DateTime.UtcNow;

        server.DataReceived += (sender, e) =>
        {
            packetCount++;

            // Print statistics every 100 packets
            if (packetCount % 100 == 0)
            {
                var elapsed = (DateTime.UtcNow - startTime).TotalSeconds;
                double packetsPerSecond = packetCount / elapsed;

                Console.WriteLine($"\n=== Statistics ===");
                Console.WriteLine($"Packets: {packetCount}");
                Console.WriteLine($"Rate: {packetsPerSecond:F2} packets/sec");
                Console.WriteLine($"Server: {server.GetStatistics()}");
                Console.WriteLine($"Valid Points: {e.GetValidDataBlockCount()}/150");
            }

            // Packet automatically returned to pool after handler completes
        };

        server.ErrorOccurred += (sender, e) =>
        {
            Console.WriteLine($"[ERROR] {e.Message}");
        };

        server.Start();

        Console.WriteLine($"Advanced LiDAR monitoring started on port {port}");
        Console.WriteLine("Press any key to stop...");
        Console.ReadKey();

        server.Stop();

        Console.WriteLine("\n=== Final Statistics ===");
        Console.WriteLine(server.GetStatistics());
        var totalTime = (DateTime.UtcNow - startTime).TotalSeconds;
        Console.WriteLine($"Total Time: {totalTime:F2} seconds");
        Console.WriteLine($"Average Rate: {packetCount / totalTime:F2} packets/sec");
    }

    /// <summary>
    /// Example with distance filtering
    /// Only process points within certain distance range
    /// </summary>
    public static void DistanceFilterExample()
    {
        const double minDistance = 100.0;  // mm
        const double maxDistance = 5000.0; // mm

        using var server = new OleiLidarServer(2368);

        server.DataReceived += (sender, e) =>
        {
            var packet = e;
            byte distanceScale = packet.Header.DistanceScale;

            foreach (var block in packet.GetValidDataBlocks())
            {
                double distance = block.GetDistance(distanceScale);

                // Filter by distance
                if (distance >= minDistance && distance <= maxDistance)
                {
                    double angle = block.GetAngleDegrees();
                    Console.WriteLine($"Filtered Point - Angle: {angle:F2}°, Distance: {distance:F2}mm");
                }
            }

            // Packet automatically returned to pool
        };

        server.Start();
        Console.WriteLine($"Distance filter active: {minDistance}-{maxDistance}mm");
        Console.WriteLine("Press any key to stop...");
        Console.ReadKey();
    }

    /// <summary>
    /// Example with angle sector filtering
    /// Only process points within certain angle range
    /// </summary>
    public static void AngleSectorExample()
    {
        const double minAngle = 45.0;   // degrees
        const double maxAngle = 135.0;  // degrees

        using var server = new OleiLidarServer(2368);

        server.DataReceived += (sender, e) =>
        {
            var packet = e;
            byte distanceScale = packet.Header.DistanceScale;

            foreach (var block in packet.GetValidDataBlocks())
            {
                double angle = block.GetAngleDegrees();

                // Filter by angle
                if (angle >= minAngle && angle <= maxAngle)
                {
                    double distance = block.GetDistance(distanceScale);
                    Console.WriteLine($"Sector Point - Angle: {angle:F2}°, Distance: {distance:F2}mm");
                }
            }

            // Packet automatically returned to pool
        };

        server.Start();
        Console.WriteLine($"Angle sector active: {minAngle}°-{maxAngle}°");
        Console.WriteLine("Press any key to stop...");
        Console.ReadKey();
    }
}