using System.Threading.Channels;
using GroundCompanionLib.Services;
using GroundCompanionLib.Services.Factories;
using GroundCompanionLib.Services.Protocols;
using Microsoft.Extensions.Hosting;

namespace GroundCompanionLib;

public class GroundCompanion(IProtocol protocol, IEnumerable<IInboundPacketHandler> inboundMavlinkPacketHandlers) : BackgroundService
{
    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
    {
        var parser = new MAVLink.MavlinkParse();
        var factory = new InboundMavlinkPacketFactory(parser);
        System.Console.WriteLine("Listening for Mavlink packets...");

        while (!stoppingToken.IsCancellationRequested)
        {
            var byt = await protocol.ReceiveAsync(stoppingToken);
            var packet = factory.AddPacketByte(byt);

            if (packet == null)
                continue;

            foreach (var handler in inboundMavlinkPacketHandlers)
            {
                handler.OnMessageReceived(packet);
            }
        }
    }

    public async Task Takeoff(CancellationToken cancellationToken)
    {
        System.Console.WriteLine("Attempting takeoff");
        var parser = new MAVLink.MavlinkParse();

        var cmd = new MAVLink.mavlink_command_long_t
        {
            command = (ushort)MAVLink.MAV_CMD.COMPONENT_ARM_DISARM,
            param1 = 1, // Arm
        };
        var packet = parser.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
        await protocol.SendAsync(packet, cancellationToken);

        await Task.Delay(300, cancellationToken);

        cmd = new MAVLink.mavlink_command_long_t
        {
            command = (ushort)MAVLink.MAV_CMD.TAKEOFF,
            param7 = 10, // Altitude
        };
        packet = parser.GenerateMAVLinkPacket20(MAVLink.MAVLINK_MSG_ID.COMMAND_LONG, cmd);
        await protocol.SendAsync(packet, cancellationToken);

        System.Console.WriteLine("Took off");
    }

    public Task Land(CancellationToken cancellationToken)
    {
        throw new NotImplementedException();
    }

    public override void Dispose()
    {
        protocol.Dispose();
        base.Dispose();
        GC.SuppressFinalize(this);
    }
}