using System;
using System.Collections.Generic;
using ClumsyCore.Pilot;
using MDCSToolBox.Commons.Controllers;

namespace MultiWheelC
{
    public class ClampToTarget : MovementDefinition
    {
        public float LeftClampTarget;
        public float RightClampTarget;
        public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
        public float ClampKp = PilotDefinition.Conf.ClampControlKp;
        public float ClampKi = PilotDefinition.Conf.ClampControlKi;
        public float ClampKd = PilotDefinition.Conf.ClampControlKd;
        public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
        public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
        public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
        public float TimeoutSeconds = 30f;
        private PIDController leftpid, rightpid;

        // C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
        public override IEnumerable<bool> Get()
        {
            try
            {
                leftpid = new PIDController(
                    () => PilotDefinition.Self.ActualPosLeftArm,
                    ClampKp, ClampKi, ClampKd, ClampMaxI,
                    ClampDeadZone, MaxClampSpeed)
                {
                    SpeedAccPerSec = ClampSpeedAcc
                };

                rightpid = new PIDController(
                    () => PilotDefinition.Self.ActualPosRightArm,
                    ClampKp, ClampKi, ClampKd, ClampMaxI,
                    ClampDeadZone, MaxClampSpeed)
                {
                    SpeedAccPerSec = ClampSpeedAcc
                };

                var startTime = DateTime.UtcNow;
                while (true)
                {
                    if (TimeoutSeconds > 0f &&
                        (DateTime.UtcNow - startTime).TotalSeconds >
                        TimeoutSeconds)
                    {
                        Console.WriteLine(
                            $"夹臂运动超时({TimeoutSeconds:F1}s)，" +
                            "停止左右夹臂。");
                        yield break;
                    }

                    var leftspeed =
                        leftpid.GetResponse(LeftClampTarget);
                    var rightspeed =
                        rightpid.GetResponse(RightClampTarget);
                    Console.WriteLine(
                        $"left arm speed:{leftspeed}  " +
                        $"right arm speed:{rightspeed}");

                    PilotDefinition.Self.SpeedLeftArm = leftspeed;
                    PilotDefinition.Self.SpeedRightArm = rightspeed;

                    var leftArrived = leftpid.IsArrived();
                    var rightArrived = rightpid.IsArrived();
                    if (leftArrived)
                        PilotDefinition.Self.SpeedLeftArm = 0f;
                    if (rightArrived)
                        PilotDefinition.Self.SpeedRightArm = 0f;

                    if (leftArrived && rightArrived)
                        break;

                    yield return true;
                }

                Console.WriteLine(
                    $"left clamp to target:{LeftClampTarget}  " +
                    $"right clamp to target:{RightClampTarget}");
            }
            finally
            {
                PilotDefinition.Self.SpeedLeftArm = 0f;
                PilotDefinition.Self.SpeedRightArm = 0f;
            }
        }
    }
}