using System;
using System.Collections.Generic;
using System.Numerics;
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using FundamentalLib;
using CommonUsage.Chassis;
using CommonUsage.Mathematics;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;

namespace MultiWheelC;

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

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

        chassis.SendMotion(0, 0, 0);
    }
}

// 原地旋转到指定世界坐标系朝向：先把舵轮打到旋转所需角度，对齐后再旋转，按目标角度停止（非固定时长）。
public class MultiRotateToWorldAngle : MovementDefinition
{
    /// <summary>目标朝向（世界坐标系，单位 deg）。</summary>
    public float TargetWorldDeg;

    /// <summary>旋转角速度（deg/s，逆时针为正）。</summary>
    public float RotSpeed = 30f;

    /// <summary>到位角度精度（deg）。</summary>
    public float ArriveDeg = 1f;

    /// <summary>起转前舵轮对齐精度（deg）。</summary>
    public float WheelAlignDeg = 2f;

    public override IEnumerable<bool> Get()
    {
        var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
        chassis.SetOriginBias(0, 0, 0);

        // 阶段一：仅把舵轮打到原地旋转所需角度（下发 0 速度，只对齐不旋转）。
        while (true)
        {
            chassis.SendRotateMotion(0);
            if (WheelsAligned(chassis, WheelAlignDeg)) break;
            yield return true;
        }

        // 阶段二：旋转到目标世界朝向，到位即停。
        var target = CommonMath.RoundTh(TargetWorldDeg);
        while (true)
        {
            var cur = CommonMath.RoundTh((float)DetourInterface.getCartLocation().th);
            var diff = CommonMath.ThDiff(target, cur); // 逆时针为正
            if (Math.Abs(diff) <= ArriveDeg) break;
            chassis.SendRotateMotion(Math.Sign(diff) * RotSpeed);
            yield return true;
        }

        chassis.PredefinedDriveStop();
    }

    private static bool WheelsAligned(MultiWheelChassis chassis, float tolDeg)
    {
#pragma warning disable CS0612, CS0618
        var wheels = chassis.GetSteerWheels();
#pragma warning restore CS0612, CS0618
        foreach (var sw in wheels)
            if (Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle())) > tolDeg)
                return false;
        return true;
    }
}

[MovementTest(name = "多舵轮-前进2秒")]
public class MultiForwardMovementTest : MovementTest
{
    private DriveTask _task;

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

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

[MovementTest(name = "多舵轮-原地旋转到目标角度")]
public class MultiRotateMovementTest : MovementTest
{
    private DriveTask _task;

    public override void Test()
    {
        _task = new DriveTask(new MultiRotateToWorldAngle
        {
            TargetWorldDeg = PilotDefinition.Conf.InPlaceRotateTargetWorldDeg,
            RotSpeed = PilotDefinition.Conf.InPlaceRotateSpeed,
            ArriveDeg = PilotDefinition.Conf.InPlaceRotateArriveDeg,
            WheelAlignDeg = PilotDefinition.Conf.InPlaceRotateWheelAlignDeg
        }.Get());
        _task.Wait();
    }

    public override void TestStop()
    {
        _task?.Stop();
        ((MultiWheelChassis)BasicPilotBase.Chassis).PredefinedDriveStop();
    }
}

// ===== 调用 Playground WebAPI 瞬移小车（前移 / 左移 / 旋转）=====
// 平移/旋转量在 Fields 面板配置：WebApiTranslateMm（默认100mm）、WebApiRotateDeg（默认5度）。

[MovementTest(name = "多舵轮-WebAPI前移")]
public class WebApiForwardMoveTest : MovementTest
{
    public override void Test()
    {
        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
        var name = PilotDefinition.Conf.PlaygroundRobotName;
        var d = PilotDefinition.Conf.WebApiTranslateMm;

        var pose = PlaygroundWebApi.GetPose(url, name);
        // 车体系前向 (d, 0) 变换到世界系：车头方向即朝向 yaw
        var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(d, 0));
        PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
        Hedingben.ToastText($"前移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiForward");
    }

    public override void TestStop()
    {
    }
}

[MovementTest(name = "多舵轮-WebAPI左移")]
public class WebApiLeftMoveTest : MovementTest
{
    public override void Test()
    {
        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
        var name = PilotDefinition.Conf.PlaygroundRobotName;
        var d = PilotDefinition.Conf.WebApiTranslateMm;

        var pose = PlaygroundWebApi.GetPose(url, name);
        // 车体系左向 (0, d) 变换到世界系（车体 +Y 即左侧）
        var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(0, d));
        PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
        Hedingben.ToastText($"左移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiLeft");
    }

    public override void TestStop()
    {
    }
}

[MovementTest(name = "多舵轮-WebAPI旋转")]
public class WebApiRotateTest : MovementTest
{
    public override void Test()
    {
        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
        var name = PilotDefinition.Conf.PlaygroundRobotName;
        var deg = PilotDefinition.Conf.WebApiRotateDeg;

        var pose = PlaygroundWebApi.GetPose(url, name);
        var ny = pose.YawDeg + deg; // 逆时针为正
        PlaygroundWebApi.Move(url, name, pose.X, pose.Y, ny);
        Hedingben.ToastText($"旋转 {deg:f1}° -> {ny:f1}°", "WebApiRotate");
    }

    public override void TestStop()
    {
    }
}

[MovementTest(name = "多舵轮-WebAPI恢复运动")]
public class WebApiMotionResumeTest : MovementTest
{
    public override void Test()
    {
        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
        PlaygroundWebApi.ResumeMotion(url);
        Hedingben.ToastText("已恢复车辆运动", "WebApiMotion");
    }

    public override void TestStop()
    {
    }
}

[MovementTest(name = "多舵轮-WebAPI暂停运动")]
public class WebApiMotionPauseTest : MovementTest
{
    public override void Test()
    {
        var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
        PlaygroundWebApi.PauseMotion(url); // 默认 zero 模式：反馈归零
        Hedingben.ToastText("已暂停车辆运动 (zero)", "WebApiMotion");
    }

    public override void TestStop()
    {
    }
}

[MovementTest(name = "多舵轮-车队直线1m")]
public class FleetStraightMovementTest : MovementTest
{
    public override void Test()
    {
        if (PilotDefinition.Conf.MultiVehicleMasterEndpoint != "/")
        {
            Hedingben.ToastText("仅主车可发起车队轨迹", "FleetGo");
            return;
        }

        var pos = DetourInterface.getCartLocation();
        var agv = new AGV();
        agv.FleetGo((float)pos.x, (float)pos.y, 1, (float)pos.x + 1000f, (float)pos.y, 2, 10001, 0.2f);
    }

    public override void TestStop()
    {
    }
}