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;

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 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 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;
            while (true)
            {
                var speed = pid.GetResponse(Target);
                Console.WriteLine($"output: {speed}  current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
                var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
                chassis.SendXYThSpeed(speed, 0, 0);
                //if (Math.Abs(current - Target) < pid.DeadZone)
                //{
                //    chassis.SendXYThSpeed(0f, 0f, 0f);
                //    Console.WriteLine($"调整退出:当前({current:f2}) ,目标:({Target:f2})");
                //    break;
                //}
                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 override IEnumerable<bool> Get()
        {
            Console.WriteLine("驱动器上使能");
            PilotDefinition.Self.ResetFromC = true;
            Thread.Sleep(100);
            PilotDefinition.Self.ResetFromC = false;
            Console.WriteLine("驱动器上使能完成");
            yield return false;
        }
    }

    public class DriverDisable : MovementDefinition
    {
        public override IEnumerable<bool> Get()
        {
            Console.WriteLine("驱动器下使能");
            PilotDefinition.Self.DisableFromC = true;
            Thread.Sleep(100);
            PilotDefinition.Self.DisableFromC = false;
            Console.WriteLine("驱动器下使能完成");
            yield return false;
        }
    }
}