using ClumsyCore;
using ClumsyCore.DTools;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using ClumsyCore.Sensors;
using ClumsyCore.Utilities;
using ClumsyDance.ClumsyWalk.Detectors;
using ClumsyDance.Sensors;
using CommonUsage.Chassis;
using FundamentalLib;
using MDCSToolBox.Clumsy.Calibration;
using MDCSToolBox.Clumsy.HighLevelSecurity;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
using MDCSToolBox.Clumsy.Tracks;
using MDCSToolBox.Commons;
using MDCSToolBox.Commons.Controllers;
using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Net.Http;
using System.Numerics;
using System.Reflection;
using System.Text;
using System.Threading;
using static ClumsyCore.DTools.Painter;

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

        public float MaxSpeed;

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

        public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;

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

        public PIDController thPid;

        private static float RangeAngle(float theta)
        {
            return (float)(theta - Math.Round(theta / 360.0f) * 360);
        }

        public override IEnumerable<bool> Get()
        {
            var targetAngle = RangeAngle(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);

            DateTime lastTime = DateTime.Now;
            while (true)
            {
                var s = thPid.GetResponse(targetAngle, true);
                Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
                Chassis.SendXYThSpeed(0, 0, s);
                lastTime = DateTime.Now;
                if (thPid.IsArrived()) break;
                yield return true;
            }
            Chassis.SendXYThSpeed(0, 0, 0);
            Console.WriteLine($"final rotate to {targetAngle}");
        }
    }


    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;
        private PIDController leftpid, rightpid;

        public override IEnumerable<bool> Get()
        {
            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 };

            while (true)
            {
                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;
                if (leftpid.IsArrived()) PilotDefinition.Self.SpeedLeftArm = 0;
                if (rightpid.IsArrived()) PilotDefinition.Self.SpeedRightArm = 0;

                if (leftpid.IsArrived() && rightpid.IsArrived()) break;
                yield return true;
            }

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

    public class Sleep : MovementDefinition
    {
        public float Second = 2;
        public override IEnumerable<bool> Get()
        {
            var start = DateTime.Now;
            while ((DateTime.Now-start).TotalSeconds<Second)
            {
                yield return true;
                Thread.Sleep(1000);
                Console.WriteLine("Sleep");
            }

            yield return false;
        }
    }

    //直线行走基于detour
    public class LineTracking1 : MovementDefinition
    {
        public float LineDistance = 1000f;
        public int SrcId = -1;
        public int DstId = -1;
        public Action<int> LeaveSrcFunction = null;
        public Painter painter = UI.GetPainter("Line", false);
        public override IEnumerable<bool> Get()
        {
            var curpose = DetourInterface.getCartLocation();
            Console.WriteLine($"curpose.th:{curpose.th}");
            var src = new Vector2((float)curpose.x, (float)curpose.y);
            var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th), 
                                  (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
            Console.WriteLine($"src:{src.X} {src.Y}");
            Console.WriteLine($"dst:{dst.X} {dst.Y}");
            painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);

            var tracker = new ChassisController().Get();
            var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
            tracker.AddTrack(linePath);
            var _dt = new DriveTask(tracker.Track());
            _dt.Wait();
            if (SrcId != -1 && LeaveSrcFunction != null)
            {
                LeaveSrcFunction(SrcId);
                DLog.Log($"释放放车点{SrcId}", "TireFollowing");
            }
            yield return false;
        }
    }

    //在世界坐标系下，从路径起点追踪到终点并停车
    public class DstTracker : MovementDefinition
    {
        public Vector2 Src;
        public Vector2 Dst;
        public float CarDirectionBias = 0f;
        public Painter Painter = UI.GetPainter("DstTracker");

        public override IEnumerable<bool> Get()
        {
            Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
            Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);

            var tracker = new ChassisController().Get();
            var linePath = new LineTrack(Src, Dst) { CarDirectionBias = CarDirectionBias, Speed = PilotDefinition.Conf.DstTrackerMaxSpeed };
            tracker.AddTrack(linePath);

            var task = new DriveTask(tracker.Track());
            task.Wait();

            // 到点后兜底停车
            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
            chassis.SendXYThSpeed(0f, 0f, 0f);
            yield return false;
        }
    }

    //直线行走基于轮里程
    public class LineTracking : MovementDefinition
    {
        public float Target;
        public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
        public float Kp = PilotDefinition.Conf.LineTrackKp;
        public float Ki = PilotDefinition.Conf.LineTrackKi;
        public float Kd = PilotDefinition.Conf.LineTrackKd;
        public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
        public int SrcId = -1;
        public int DstId = -1;
        public Action<int> LeaveSrcFunction = null;
        private PIDController pid;

        public override IEnumerable<bool> Get()
        {

            pid = new PIDController(() =>
                (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
                Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
            { SpeedAccPerSec = MaxSpeed / 2f };

            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
            //chassis.SetOriginBias(0, 0, 0);
            DLog.Log($"直线行驶距离:{Target}", "TireFollowing");
            while (true)
            {
                var speed = pid.GetResponse(Target);
                Console.WriteLine($"output: {speed}  current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
                
                chassis.SendXYThSpeed(speed, 0, 0);

                if (pid.IsArrived()) break;
                yield return true;
            }
            if (SrcId != -1 && LeaveSrcFunction != null)
            {
                LeaveSrcFunction(SrcId);
                DLog.Log($"释放放车点{SrcId}", "TireFollowing");
            }
            yield return false;
        }
    }

    public class DriverAble : MovementDefinition
    {
        public int WaitTimeoutMs = 2000;
        public int PollIntervalMs = 50;

        public override IEnumerable<bool> Get()
        {
            Console.WriteLine("驱动器上使能");
            PilotDefinition.Self.ResetFromC = true;

            var start = DateTime.Now;
            var timeoutMs = Math.Max(0, WaitTimeoutMs);
            var pollMs = Math.Max(1, PollIntervalMs);
            var success = PilotDefinition.Self.WheelAbleState;
            while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
            {
                Thread.Sleep(pollMs);
                success = PilotDefinition.Self.WheelAbleState;
                if (!success) yield return true;
            }

            PilotDefinition.Self.ResetFromC = false;
            if (success)
                Console.WriteLine($"驱动器上使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
            else
                Console.WriteLine($"驱动器上使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
            yield return false;
        }
    }

    public class DriverDisable : MovementDefinition
    {
        public int WaitTimeoutMs = 3000;
        public int PollIntervalMs = 20;

        public override IEnumerable<bool> Get()
        {
            Console.WriteLine("驱动器下使能");
            PilotDefinition.Self.DisableFromC = true;

            var start = DateTime.Now;
            var timeoutMs = Math.Max(0, WaitTimeoutMs);
            var pollMs = Math.Max(1, PollIntervalMs);
            var success = !PilotDefinition.Self.WheelAbleState;
            while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
            {
                Thread.Sleep(pollMs);
                success = !PilotDefinition.Self.WheelAbleState;
                if (!success) yield return true;
            }

            PilotDefinition.Self.DisableFromC = false;
            if (success)
                Console.WriteLine($"驱动器下使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
            else
                Console.WriteLine($"驱动器下使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
            yield return false;
        }
    }
}