using System;
using System.Collections.Generic;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using MDCSToolBox.Commons.Controllers;
using MyParking.Shared;

namespace MultiWheelC
{
    public class MultiWheelRotateInPlace : MovementDefinition
    {
        /// <summary>
        /// 旋转目标角度
        /// </summary>
        public float AngleTarget;

        public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;

        public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;

        public Func<PIDParams> PidparamsRead = () => new PIDParams() { };

        public PIDController thPid;

        // 将本周期PID角速度输出提供给实验记录器，单位deg/s。
        public Action<float> CommandAngularSpeedObserver;

        // 自转前舵轮实际角度允许误差，单位deg。
        public float WheelAlignmentToleranceDegrees = 2f;

        // 自转舵轮连续保持到位的时间，单位s。
        public float WheelAlignmentStableSeconds = 0.3f;

        // 自转舵轮准备超时时间，单位s。
        public float WheelAlignmentTimeoutSeconds = 10f;

        // 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
        public override IEnumerable<bool> Get()
        {
            if (Chassis == null)
                throw new InvalidOperationException(
                    "当前底盘不是MultiWheelChassis，无法执行原地自转。");

            var adapter = new MultiWheelChassisAdapter(
                Chassis,
                PilotDefinition.Self.CarNum);
            adapter.ResetToBodyFrame();

            try
            {
                var alignmentStarted = DateTime.Now;
                DateTime? alignedSince = null;
                while (true)
                {
                    if (!adapter.PrepareSpin())
                        throw new InvalidOperationException(
                            "无法生成原地自转舵轮目标：" +
                            adapter.LastFailureReason);

                    if (adapter.AreSpinWheelsAligned)
                    {
                        if (alignedSince == null)
                            alignedSince = DateTime.Now;

                        if ((DateTime.Now - alignedSince.Value)
                                .TotalSeconds >=
                            WheelAlignmentStableSeconds)
                            break;
                    }
                    else
                    {
                        alignedSince = null;
                    }

                    if ((DateTime.Now - alignmentStarted)
                            .TotalSeconds >
                        WheelAlignmentTimeoutSeconds)
                        throw new TimeoutException(
                            "原地自转舵轮在限定时间内未稳定到位。");

                    yield return true;
                }

                var targetAngle =
                    (float)AngleMath.NormalizeDegrees(AngleTarget);
                var p = PidparamsRead();
                thPid = new PIDController(ThetaReader, p.Kp);
                thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
                    p.OutputUpperThreshold, p.SpeedAccPerSec);
                var lastCommandTime = DateTime.Now;

                while (true)
                {
                    var s = thPid.GetResponse(targetAngle, true);
                    Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
                    CommandAngularSpeedObserver?.Invoke(s);
                    var now = DateTime.Now;
                    var interval = now - lastCommandTime;
                    lastCommandTime = now;

                    // PID输出s为deg/s，Shared命令统一使用rad/s。
                    // adapter.Send最终调用普通安全版SendXYThSpeed。
                    var omegaRadiansPerSecond =
                        (float)AngleMath.DegreesToRadians(s);
                    if (!adapter.Send(
                            new ChassisCommand(
                                PilotDefinition.Self.CarNum,
                                new Twist2D(
                                    0.0,
                                    0.0,
                                    omegaRadiansPerSecond)),
                            interval))
                    {
                        throw new InvalidOperationException(
                            "安全XYTh原地旋转底盘解算失败：" +
                            adapter.LastFailureReason);
                    }
                    if (thPid.IsArrived()) break;
                    yield return true;
                }

                Console.WriteLine($"final rotate to {targetAngle}");
            }
            finally
            {
                CommandAngularSpeedObserver?.Invoke(0f);
                adapter.StopImmediately();
            }
        }
    }
}