using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Sensors;
using CommonUsage.Chassis;
using CommonUsage.Mathematics;
using FundamentalLib;
using MDCSToolBox.Clumsy.AgvInterfaces;
using MDCSToolBox.Clumsy.Calibration;
using MDCSToolBox.Clumsy.MotionControllers;
using MDCSToolBox.Clumsy.Tracks;
using MDCSToolBox.Commons.Controllers;
using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using System.Net.Http;
using System.Numerics;
using System.Security.Cryptography;
using System.Threading;
using System.Threading.Tasks;
using static ClumsyCore.DTools.Painter;

namespace MultiWheelC
{
    public class SetLocationRes
    {
        public float x, y, th;
        public int l_step;
        public long tick;
        public string error;
    }
    public class AGV : MultiWheelInterface
    {
        public override AbstractGeometricController GetController()
        {
            return new ChassisController().Get();
        }

        public override MultiWheelMagTracker GetMagController()
        {
            return new MultiWheelMagTracker();
        }

        public override NaiveMagnetController GetNaiveMagnetController()
        {
            return new NaiveMagnetController();
        }

        public void Sleep(float s)
        {
            new DriveTask(new Sleep() { Second = s }.Get()).Wait();
        }

        public void ControlChargePort(bool open)
        {
            DLog.Log($"call ControlChargePort({open})");
            PilotDefinition.Self.OpenChargeByClumsy = open;
        }

        public void SwitchLidarArea(int area)
        {
            DLog.Log($"call SwitchLidarArea({area})");
            PilotDefinition.Self.AreaChoose = area;
        }

        public void SwitchIoArea(int area)
        {
            if (area != -1)
            {
                PilotDefinition.Self.IOObstacleArea = area;
            }
        }
        public void RotateToTarget(float target)
        {
            //if (!needrotate) return;
            var dl = new DriveTask(new MultiWheelRotateInPlace()
            {
                AngleTarget = target,
                PidparamsRead = () => new PIDParams()
                {
                    Kp = PilotDefinition.Conf.TireFollowingThkp,
                    Ki = PilotDefinition.Conf.TireFollowingThki,
                    Kd = PilotDefinition.Conf.TireFollowingThkd,
                    DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
                    SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
                    OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
                    MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
                }
            }.Get());
            dl.Wait();
        }

        //参数1：tireNum 需要钻过的轮胎对数量
        //参数2：frontLidarDetect true:前雷达识别  false:后雷达识别
        public void TireFollowing(int tireNum, bool frontLidarDetect, int srcId, int dstId)
        {
            while (!TryLock(dstId))
            {
                Thread.Sleep(50);
            }
            DLog.Log($"锁点{dstId}完成", "TireFollowing");
            var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
            DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
            if (tireNum != 1 && tireNum != 2)
            {
                DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
                return;
            }

            if (PilotDefinition.Self.GhostMode)
            {
                while (!TryLock(dstId))
                {
                    Console.WriteLine("等待锁取货点中...");
                    Thread.Sleep(200);
                }
                Console.WriteLine($"锁点{dstId}完成");
                Thread.Sleep(1000);
                Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
                Thread.Sleep(1000);
                Leave(srcId);
                Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
                Thread.Sleep(2000);
                //Leave(dstId);
                //Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
                Thread.Sleep(2000);
                Console.WriteLine($"结束钻车动作");
                return;
            }

            var detectors = new List<TireFollowing.DetectorDefinition>()
            {
                new TireFollowing.DetectorDefinition()
                {
                    DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
                    StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
                    StartGuessingY = 0,
                    SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
                    FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
                    PathTransformation = new Tuple<float, float, float>(
                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
                    0),
                    LeaveSrcFunction = Leave,
                    SrcId = srcId,
                    DstId = dstId,
                },
                new TireFollowing.DetectorDefinition()
                {
                    DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
                    StartGuessingX = frontLidarDetect ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
                    StartGuessingY = 0,
                    SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
                    FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
                    PathTransformation = new Tuple<float, float, float>(
                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
                    frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
                    0)
                },
            };
            DLog.Log($"钻胎为{tireNum}", "TireFollowing");
            var following = new TireFollowing()
            {
                GetController = () => new ChassisController().Get(),
                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
                detectors = detectors,
                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
                MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
                TireNum = tireNum,
                CarDirection = frontLidarDetect ? 0f : 180f,
                WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
            };
            var _dt = new DriveTask(following.Get());
            _dt.Wait();
            DLog.Log("钻车动作结束", "TireFollowing");
        }

        //离车一定是后雷达识别一个轮胎
        public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
        {
            while (!TryLock(dstId))
            {
                Thread.Sleep(50);
            }
            DLog.Log($"锁点{dstId}完成", "TireFollowing");
            DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
            chassis.SetOriginBias(0, 0, 0);
            var following = new TireFollowing()
            {
                GetController = () => new ChassisController().Get(),
                GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
                GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
                detectors = new List<TireFollowing.DetectorDefinition>()
                {
                    new TireFollowing.DetectorDefinition()
                    {
                        DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
                        StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
                        StartGuessingY = 0,
                        SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
                        FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
                        PathTransformation = new Tuple<float, float, float>(
                        PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
                        PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
                        0),
                        LeaveSrcFunction = Leave,
                        SrcId = srcId,
                        DstId = dstId,
                    },
                },
                CarDirection = 180f,
                SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
                MaxSpeed = 0.25f,
                EnableHandover = true,
                HandoverDistance = 200f,
                HandoverSpeed = 0.3f,
                WalkBlindTh = 0,
                TireNum = 1
            };

            IEnumerable<bool> LeaveThenFollow()
            {
                foreach (var running in following.Get())
                {
                    if (!running) break;
                    yield return true;
                }
                DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
                DLog.Log("离车TireFollowing结束，开始DstTracker", "TireFollowing");

                foreach (var running in new DstTracker()
                {
                    Src = new Vector2(srcX, srcY),
                    Dst = new Vector2(dstX, dstY),
                    CarDirectionBias = 180f,
                    InitialSendSpeed = 0.3f
                }.Get())
                {
                    if (!running) break;
                    yield return true;
                }

                yield return false;
            }

            var _dt = new DriveTask(LeaveThenFollow());
            _dt.Wait();
            DLog.Log("离车动作1结束", "TireFollowing");
        }

        public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
        {
            while (!TryLock(dstId))
            {
                Thread.Sleep(50);
            }
            var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
            chassis.SetOriginBias(0, 0, 0);
            DLog.Log($"锁点{dstId}完成", "TireFollowing");
            IEnumerable<bool> TrackThenFollow()
            {
                foreach (var running in new LineTracking()
                {
                    Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
                    LeaveSrcFunction = Leave,
                    SrcId = srcId,
                    EnableHandover = true,
                    HandoverDistance = 200, 
                    HandoverSpeed = 0.3f,
                }.Get())
                {
                    if (!running) break;
                    yield return true;
                }
                DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
                DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
                while (!TryLock(426))
                {
                    Thread.Sleep(20);
                }
                Leave(dstId);
                DLog.Log($"释放锁点{dstId}完成", "TireFollowing");
                foreach (var running in new DstTracker()
                {
                    Src = new Vector2(srcX, srcY),
                    Dst = new Vector2(dstX, dstY),
                    InitialSendSpeed = 0.3f
                }.Get())
                {
                    if (!running) break;
                    yield return true;
                }
                yield return false;
            }
            var _dt = new DriveTask(TrackThenFollow());
            _dt.Wait();
            DLog.Log("离车动作2结束", "TireFollowing");
        }

        //驱动器上使能
        public void DriverAble()
        {
            var dl = new DriveTask(new DriverAble() { }.Get());
            dl.Wait();
            DLog.Log("驱动器上使能完成", "TireFollowing");
        }

        //驱动器下使能
        public void DriverDisable()
        {
            var dl = new DriveTask(new DriverDisable() { }.Get());
            dl.Wait();
            DLog.Log("驱动器下使能完成", "TireFollowing");
        }

        // 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
        public void ClamptoTarget(bool close)
        {
            if (PilotDefinition.Self.GhostMode)
            {
                Thread.Sleep(2000);
                Console.WriteLine("夹抱完成");
                return;
            }
            new DriveTask(new ClampToTarget()
            {
                LeftClampTarget = close ? PilotDefinition.Self.LeftArmUpperPos : PilotDefinition.Self.LeftArmLowerPos,
                RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
            }.Get()).Wait();
        }
        // Fleet crab walk: convert scheduler src/dst into the same relative crab-walk path used by MovementTest.
        public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
            float speed)
        {
            var dx = dstX - srcX;
            var dy = dstY - srcY;
            var pathLength = (float)Math.Sqrt(dx * dx + dy * dy);

            if (pathLength <= 1f)
            {
                DLog.Log("FleetCrabWalk abort: path length is too short.", "FleetCrabDbg");
                return;
            }

            var self = PilotDefinition.Self;
            if (!self.TryGetFleetCenterFromMembers(out var centerX, out var centerY, out var centerTh) &&
                !self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
            {
                DLog.Log("FleetCrabWalk abort: failed to read fleet center.", "FleetCrabDbg");
                Hedingben.ToastText("FleetCrab requires master localization", "FleetCrab");
                return;
            }

            var pathAngle = (float)CommonMath.RoundTh((float)(Math.Atan2(dy, dx) / Math.PI * 180.0));
            var crabAngle = (float)CommonMath.ThDiff(pathAngle, centerTh);
            var targetBodyWorldHeading = (float)CommonMath.RoundTh(PilotDefinition.Conf.FleetCrabBodyWorldHeadingDeg);
            var bodyToPathAngle = (float)CommonMath.ThDiff(pathAngle, targetBodyWorldHeading);
            DLog.Log(
                $"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
                $"len={pathLength:0.0}, speed={speed:0.000}, pathAngle={pathAngle:0.0}, " +
                $"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0}, " +
                $"targetBodyWorld={targetBodyWorldHeading:0.0}, bodyToPath={bodyToPathAngle:0.0})",
                "FleetCrabDbg");

            if (dstId != -1)
            {
                while (!TryLock(dstId))
                {
                    Thread.Sleep(50);
                }
                DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
            }

            var action = new MultiWheelC.FleetCrabWalk
            {
                CrabAngleDeg = crabAngle,
                BodyToPathAngleDeg = bodyToPathAngle,
                CrabLengthMm = pathLength,
                CrabSpeed = speed,
                FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
                FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
                FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
                FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
                FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
                FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
                GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
            };

            try
            {
                new DriveTask(action.Get()).Wait();
            }
            finally
            {
                if (srcId != -1)
                {
                    Leave(srcId);
                    DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
                }
            }
        }

        public void FleetCurveWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
            float speed, params float[] trackTypeInfo)
        {
            if (trackTypeInfo == null || trackTypeInfo.Length < 2)
            {
                DLog.Log("FleetCurveWalk abort: invalid trackTypeInfo, expected Bezier type info.", "FleetCurveDbg");
                Hedingben.ToastText("FleetCurve invalid trackTypeInfo", "FleetCurve");
                return;
            }

            var trackType = (int)trackTypeInfo[0];
            if (trackType != 2)
            {
                DLog.Log($"FleetCurveWalk abort: unsupported trackType={trackType}, only Bezier(type=2) is supported.",
                    "FleetCurveDbg");
                Hedingben.ToastText("FleetCurve only supports Bezier trackType=2", "FleetCurve");
                return;
            }

            var controlPointNum = (int)trackTypeInfo[1];
            var expectedLength = 2 + controlPointNum * 2;
            if (controlPointNum < 3 || trackTypeInfo.Length < expectedLength)
            {
                DLog.Log(
                    $"FleetCurveWalk abort: invalid Bezier trackTypeInfo. controlPointNum={controlPointNum}, " +
                    $"length={trackTypeInfo.Length}, expected>={expectedLength}.",
                    "FleetCurveDbg");
                Hedingben.ToastText("FleetCurve invalid Bezier trackTypeInfo", "FleetCurve");
                return;
            }

            BezierTrack track;
            try
            {
                track = ProcessTrackTypeInfo(srcX, srcY, dstX, dstY, trackTypeInfo) as BezierTrack;
            }
            catch (Exception ex)
            {
                DLog.Log($"FleetCurveWalk abort: failed to process trackTypeInfo. {ex.Message}", "FleetCurveDbg");
                Hedingben.ToastText("FleetCurve failed to process track", "FleetCurve");
                return;
            }

            if (track == null)
            {
                DLog.Log("FleetCurveWalk abort: ProcessTrackTypeInfo did not return BezierTrack.", "FleetCurveDbg");
                Hedingben.ToastText("FleetCurve requires BezierTrack", "FleetCurve");
                return;
            }

            track.Speed = speed;
            track.CarDirectionBias = 0f;

            DLog.Log(
                $"call FleetCurveWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
                $"speed={speed:0.000}, trackType={trackType}, controls={controlPointNum}, track={track.GetType().Name}, " +
                $"carDirectionBias=0.0)",
                "FleetCurveDbg");

            if (dstId != -1)
            {
                while (!TryLock(dstId))
                {
                    Thread.Sleep(50);
                }
                DLog.Log($"閿佺偣{dstId}瀹屾垚", "FleetCurveDbg");
            }

            var action = new MultiWheelC.FleetCurveWalk
            {
                Track = track,
                CurveSpeed = speed,
                CarDirectionBias = 0f,
                SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
                FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
                FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
                SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
                GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
                StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
            };

            try
            {
                new DriveTask(action.Get()).Wait();
            }
            finally
            {
                if (srcId != -1)
                {
                    Leave(srcId);
                    DLog.Log($"release srcId={srcId}", "FleetCurveDbg");
                }
            }
        }
        
        public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
        {
            DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})");
            PilotDefinition.Self.SlowDistance = slowDistance;
            PilotDefinition.Self.StopDistance = stopDistance;
        }
        public void ChangeAvoidanceParam(float length = -1, float width = -1)
        {

            PilotDefinition.Self.CarLength = length;
            PilotDefinition.Self.CarWidth = width;

        }
        public void SetLocation(float x, float y, float th)
        {
            DLog.Log($"call SetLocation({x},{y},{th})");
            Console.WriteLine($"call SetLocation({x},{y},{th})");
            Queue(() =>
            {
                while (true)
                {
                    var str1 = new HttpClient()
                        .GetStringAsync(
                            $"http://127.0.0.1:4321/setLocation?x={x}&y={y}&th={th}")
                        .Result;
                    Thread.Sleep(500);
                    Console.WriteLine($"SetLocation str={str1}");
                    var setLocationRes = JsonConvert.DeserializeObject<SetLocationRes>(str1);
                    Console.WriteLine(setLocationRes.l_step);
                    if (setLocationRes != null && setLocationRes.l_step == 2) break;
                }
            });
        }
        public float baseSpeed = 0;
    }
}