using System;
using System.Collections.Generic;
using System.Globalization;
using System.Numerics;
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Pilot;
using FundamentalLib;
using CommonUsage.Chassis;
using CommonUsage.Mathematics;
using MDCSToolBox.Clumsy.MotionControllers;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
using MDCSToolBox.Clumsy.Tracks;

namespace MultiWheelC;

public class FleetCurveWalk : MovementDefinition
{
    public BezierTrack Track;
    public List<Vector2> ControlPoints = new();
    public float CurveSpeed = 0.2f;
    public float CarDirectionBias = 0f;
    public int BezierResolution = 100;
    public float SlowDistance = 2000f;
    public float FinishDistance = 20f;
    public float FinishSpeed = 0.02f;
    public float SlowingPow = 0.8f;
    public float GcpThetaThreshold = 95f;
    public float StartSyncTimeoutSec = 8f;

    private bool _stopping;
    private MultiWheelGeometricController _controller;
    private MultiWheelChassis _chassis;
    private bool _savedControlPoints;
    private float _savedControlRadius;
    private Vector2 _savedGcp0;
    private Vector2 _savedGcp1;

    public void Stop()
    {
        _stopping = true;
        if (_controller != null)
            _controller.BreakAndHold = true;
        Cleanup();
    }

    public static bool TryParsePointList(string text, out List<Vector2> points, out string error)
    {
        points = new List<Vector2>();
        error = "";
        if (string.IsNullOrWhiteSpace(text))
        {
            error = "empty control point list";
            return false;
        }

        var segments = text.Split(new[] { ';', '|' }, StringSplitOptions.RemoveEmptyEntries);
        for (var i = 0; i < segments.Length; i++)
        {
            var pair = segments[i].Split(new[] { ',', ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
            if (pair.Length != 2)
            {
                error = $"invalid point #{i + 1}: {segments[i]}";
                return false;
            }

            if (!TryParseFloat(pair[0], out var x) || !TryParseFloat(pair[1], out var y))
            {
                error = $"invalid number in point #{i + 1}: {segments[i]}";
                return false;
            }
            points.Add(new Vector2(x, y));
        }

        if (points.Count < 3)
        {
            error = "Bezier curve requires at least 3 control points";
            return false;
        }

        return true;
    }

    public static List<Vector2> BuildRelativeControlPoints(Vector2 start, float startTh, List<Vector2> relativePoints)
    {
        var source = relativePoints ?? new List<Vector2>();
        var normalized = new List<Vector2>();
        if (source.Count == 0 || Vector2.Distance(source[0], Vector2.Zero) > 1f)
            normalized.Add(Vector2.Zero);
        for (var i = 0; i < source.Count; i++)
            normalized.Add(source[i]);
        if (normalized.Count < 2)
            normalized.Add(new Vector2(1000f, 0f));
        if (normalized.Count < 3)
            normalized.Add(new Vector2(2000f, 0f));

        var result = new List<Vector2>();
        for (var i = 0; i < normalized.Count; i++)
            result.Add(CommonMath.Transform2D(start, startTh, normalized[i]));
        return result;
    }

    public static List<Vector2> BuildAgvControlPoints(float srcX, float srcY, float dstX, float dstY,
        params float[] controlPointCoords)
    {
        var src = new Vector2(srcX, srcY);
        var dst = new Vector2(dstX, dstY);
        var result = new List<Vector2>();
        if (controlPointCoords == null || controlPointCoords.Length == 0)
        {
            result.Add(src);
            result.Add((src + dst) / 2f);
            result.Add(dst);
            return result;
        }

        if (controlPointCoords.Length % 2 != 0)
            throw new ArgumentException("FleetCurve controlPointCoords must contain x,y pairs.");

        var supplied = new List<Vector2>();
        for (var i = 0; i < controlPointCoords.Length; i += 2)
            supplied.Add(new Vector2(controlPointCoords[i], controlPointCoords[i + 1]));

        if (supplied.Count >= 3 &&
            Vector2.Distance(supplied[0], src) <= 10f &&
            Vector2.Distance(supplied[supplied.Count - 1], dst) <= 10f)
            return supplied;

        result.Add(src);
        for (var i = 0; i < supplied.Count; i++)
            result.Add(supplied[i]);
        result.Add(dst);
        if (result.Count < 3)
            result.Insert(1, (src + dst) / 2f);
        return result;
    }

    private static bool TryParseFloat(string text, out float value)
    {
        return float.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value) ||
               float.TryParse(text, out value);
    }

    private static float ClampAbs(float value, float limit)
    {
        var absLimit = Math.Abs(limit);
        if (absLimit <= 0) return value;
        if (value > absLimit) return absLimit;
        if (value < -absLimit) return -absLimit;
        return value;
    }

    private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
        out float centerTh, out string source)
    {
        if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
        {
            source = "fleet";
            return true;
        }

        if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
        {
            source = "slam";
            return true;
        }

        source = "none";
        return false;
    }

    private void Cleanup()
    {
        var self = PilotDefinition.Self;
        self.MultiVehicleScriptVx = 0;
        self.MultiVehicleScriptVy = 0;
        self.MultiVehicleScriptVth = 0;
        self.MultiVehicleScriptMode = 0;
        self.MultiVehicleScriptEnabled = false;
        self.MultiVehicleAutoVx = 0;
        self.MultiVehicleAutoFrontTh = 0;
        self.MultiVehicleAutoRearTh = 0;
        self.MultiVehicleAutoHasIdeal = false;
        self.MultiVehicleAutoEnabled = false;
        RestoreControlPointRadius();
    }

    private void ApplyFleetControlPointRadius(MultiWheelChassis chassis, float radius)
    {
        if (!_savedControlPoints)
        {
            _chassis = chassis;
            _savedControlRadius = chassis.ControlPointRadius;
            var gcps = chassis.GetGeometricControlPoints();
            if (gcps.Count >= 2)
            {
                _savedGcp0 = gcps[0].Position;
                _savedGcp1 = gcps[1].Position;
            }
            _savedControlPoints = true;
        }

        chassis.ControlPointRadius = radius;
        var points = chassis.GetGeometricControlPoints();
        if (points.Count >= 2)
        {
            points[0].Position = new Vector2(radius, 0);
            points[1].Position = new Vector2(-radius, 0);
        }
    }

    private void RestoreControlPointRadius()
    {
        if (!_savedControlPoints || _chassis == null)
            return;

        _chassis.ControlPointRadius = _savedControlRadius;
        var points = _chassis.GetGeometricControlPoints();
        if (points.Count >= 2)
        {
            points[0].Position = _savedGcp0;
            points[1].Position = _savedGcp1;
        }
        _savedControlPoints = false;
    }

    private static void WriteWarmupAuto(PilotDefinition self, Vector2 idealPos, float idealTh,
        float frontTh, float rearTh)
    {
        self.MultiVehicleScriptEnabled = false;
        self.MultiVehicleScriptMode = 0;
        self.MultiVehicleScriptVx = 0;
        self.MultiVehicleScriptVy = 0;
        self.MultiVehicleScriptVth = 0;
        self.MultiVehicleAutoEnabled = true;
        self.MultiVehicleAutoVx = 0;
        self.MultiVehicleAutoFrontTh = frontTh;
        self.MultiVehicleAutoRearTh = rearTh;
        self.MultiVehicleAutoIdealX = idealPos.X;
        self.MultiVehicleAutoIdealY = idealPos.Y;
        self.MultiVehicleAutoIdealTh = idealTh;
        self.MultiVehicleAutoHasIdeal = true;
        self.MultiVehicleAutoCmdTime = DateTime.Now;
    }

    public override IEnumerable<bool> Get()
    {
        var self = PilotDefinition.Self;
        var conf = PilotDefinition.Conf;
        var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
        _stopping = false;

        if (chassis == null)
        {
            DLog.Log("ABORT: FleetCurveWalk requires MultiWheelChassis.", "FleetCurveDbg");
            yield break;
        }

        if (conf.MultiVehicleMasterEndpoint != "/")
        {
            DLog.Log($"ABORT: FleetCurveWalk must run on master endpoint, endpoint={conf.MultiVehicleMasterEndpoint}",
                "FleetCurveDbg");
            Hedingben.ToastText("FleetCurve requires master vehicle", "FleetCurve");
            yield break;
        }

        if (Track == null && (ControlPoints == null || ControlPoints.Count < 3))
        {
            DLog.Log("ABORT: FleetCurveWalk requires a BezierTrack or at least 3 control points.", "FleetCurveDbg");
            Hedingben.ToastText("FleetCurve requires track or >=3 control points", "FleetCurve");
            yield break;
        }

        if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
        {
            DLog.Log("ABORT: FleetCurveWalk failed to read fleet center.", "FleetCurveDbg");
            Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
            yield break;
        }

        var baseSpeed = Math.Abs(CurveSpeed);
        if (baseSpeed <= 1e-4f)
        {
            DLog.Log("ABORT: FleetCurveWalk speed is zero.", "FleetCurveDbg");
            yield break;
        }

        var resolution = Math.Max(2, BezierResolution);
        var speedFinish = Math.Min(baseSpeed, Math.Abs(FinishSpeed));
        var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
        var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);

        ApplyFleetControlPointRadius(chassis, controlRadius);

        try
        {
            var track = Track;
            var trackSource = "external";
            if (track == null)
            {
                var points = new List<Vector2>(ControlPoints);
                track = new BezierTrack(points, resolution);
                trackSource = "controlPoints";
            }
            track.CarDirectionBias = CarDirectionBias;
            track.Speed = baseSpeed;

            var center = new Vector2(x0, y0);
            var (idealPos, idealAngle, bias, pd) = track.QueryTangentPoint(center);
            var carDirection = (float)CommonMath.ThDiff(theta, CarDirectionBias);
            var holdTh = ClampAbs((float)CommonMath.ThDiff(idealAngle, carDirection), gcpLimit);
            var targetBodyTh = (float)CommonMath.RoundTh(idealAngle + CarDirectionBias);

            DLog.Log(
                $"START center=({x0:0},{y0:0},{theta:0.0}) source={initialCenterSource} " +
                $"track={track.GetType().Name} trackSource={trackSource} controls={ControlPoints?.Count ?? 0} " +
                $"len={track.Length():0} speed={baseSpeed:0.000} bias={CarDirectionBias:0.0} " +
                $"query=({idealPos.X:0},{idealPos.Y:0}) tangent={idealAngle:0.0} targetBody={targetBodyTh:0.0} " +
                $"pathBias={bias:0.0} pd={pd:0.0} hold={holdTh:0.0} radius={controlRadius:0}",
                "FleetCurveDbg");

            var warmStart = DateTime.Now;
            var warmSeqBaseline = self.BeginFleetMotionWarmup();
            WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
            self.PrimeMasterAutoFromSlam();

            var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, StartSyncTimeoutSec));
            var warmIter = 0;
            var warmReady = false;
            var warmDetail = "";
            while (!_stopping && DateTime.Now < warmEnd)
            {
                warmIter++;
                WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
                self.PrimeMasterAutoFromSlam();

                if (warmIter % 5 == 0)
                {
                    var snap = self.GetFleetCenterSnapshot();
                    int cnt;
                    lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
                    DLog.Log(
                        $"WARMUP#{warmIter} snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
                        $"cnt={cnt}/{conf.MultiVehicleFleetNum} detail={warmDetail}",
                        "FleetCurveDbg");
                }

                if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
                        conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
                {
                    warmReady = true;
                    DLog.Log($"WARMUP done iter={warmIter} detail={warmDetail}", "FleetCurveDbg");
                    break;
                }
                yield return true;
            }

            if (!warmReady)
            {
                DLog.Log($"WARMUP timeout: fleet startup sync failed, abort curve action. detail={warmDetail}",
                    "FleetCurveDbg");
                Hedingben.ToastText("FleetCurve startup sync timeout", "FleetCurve");
                Cleanup();
                yield break;
            }

            _controller = new ChassisController { BaseSpeed = baseSpeed }.Get();
            _controller.MultiVehicleSync = true;
            _controller.BaseSpeed = baseSpeed;
            _controller.SlowDistance = Math.Max(FinishDistance + 1f, SlowDistance);
            _controller.FinishDistance = Math.Max(0f, FinishDistance);
            _controller.FinishSpeed = speedFinish;
            _controller.SlowingPow = Math.Max(0.01f, SlowingPow);
            _controller.GcpThetaThreshold = gcpLimit;
            _controller.AddTrack(track, "FleetCurve");

            Hedingben.ToastText($"FleetCurve len {track.Length():0}mm speed {baseSpeed:0.00}", "FleetCurve");

            foreach (var running in _controller.Track())
            {
                if (_stopping)
                    break;
                if (!running)
                    break;
                yield return true;
            }

            if (!_stopping)
            {
                self.MultiVehicleAutoVx = 0;
                self.MultiVehicleAutoCmdTime = DateTime.Now;
                var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
                while (!_stopping && DateTime.Now < settleEnd)
                {
                    self.MultiVehicleAutoEnabled = true;
                    self.MultiVehicleAutoVx = 0;
                    self.MultiVehicleAutoCmdTime = DateTime.Now;
                    yield return true;
                }
            }

            DLog.Log($"DONE stopping={_stopping}", "FleetCurveDbg");
        }
        finally
        {
            Cleanup();
            _controller = null;
        }
    }
}