using System;
using System.Collections.Generic;
using System.Threading;
using ClumsyCore;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;

namespace DiffWheelC;

public class DiffForwardTest : MovementDefinition
{
    public float Speed = 0.2f;
    public float DurationSeconds = 2f;

    public override IEnumerable<bool> Get()
    {
        var chassis = (DifferentialChassis)BasicPilotBase.Chassis;
        var end = DateTime.Now.AddSeconds(DurationSeconds);
        while (DateTime.Now < end)
        {
            chassis.SendSpeed(Speed, 0);
            yield return true;
        }

        chassis.SendSpeed(0, 0);
    }
}

public class DiffRotateTest : MovementDefinition
{
    public float AngularSpeed = 30f;
    public float DurationSeconds = 2f;

    public override IEnumerable<bool> Get()
    {
        var chassis = (DifferentialChassis)BasicPilotBase.Chassis;
        var end = DateTime.Now.AddSeconds(DurationSeconds);
        while (DateTime.Now < end)
        {
            chassis.SendSpeed(0, AngularSpeed);
            yield return true;
        }

        chassis.SendSpeed(0, 0);
    }
}

[MovementTest(name = "差速-前进2秒")]
public class DiffForwardMovementTest : MovementTest
{
    private DriveTask _task;

    public override void Test()
    {
        _task = new DriveTask(new DiffForwardTest().Get());
        _task.Wait();
    }

    public override void TestStop() => _task?.Stop();
}

[MovementTest(name = "差速-原地旋转2秒")]
public class DiffRotateMovementTest : MovementTest
{
    private DriveTask _task;

    public override void Test()
    {
        _task = new DriveTask(new DiffRotateTest().Get());
        _task.Wait();
    }

    public override void TestStop() => _task?.Stop();
}