using ClumsyCore;
using MDCSToolBox.Clumsy.Pilot.MultiWheel;
using Newtonsoft.Json;

namespace MultiWheelC;

public class PilotConfig : MultiWheelPilotConfig
{
    // ===== 多车联动（已从 MDCSToolBox 内联回 Tutorial）=====
    [FieldMember(desc = "[sync] 转向角加速度(deg/s^2)")] public float SyncThAccPerSec = 30f;
    [FieldMember(desc = "[sync] 编队车间距(mm)")] public float TestCarSyncDistance = 2400f;
    [FieldMember(desc = "[sync] 编队布局偏角(deg)")] public float TestCarSyncTh = 0f;
    // 手动遥控 Vx 已是 m/s、Vth 已是转向角(deg)，此处系数保持 1（直通），不要再次缩放。
    [FieldMember(desc = "[sync] 手动Vx系数")] public float ManualCarSyncVxFac = 1f;
    [FieldMember(desc = "[sync] 手动Vy系数(蟹行横向)")] public float ManualCarSyncVyFac = 1f;
    [FieldMember(desc = "[sync] 手动Vth系数")] public float ManualCarSyncVthFac = 1f;
    [FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
    [FieldMember(desc = "[sync] 是否启用Detour定位")] public bool PosAvailable = true;

    [FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
    [FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
    [FieldMember(desc = "多车联动：主车端点 ip:port，/ 表示本车为主车")] public string MultiVehicleMasterEndpoint = "/";
    [FieldMember(desc = "多车联动：本车同步 IP")] public string SimpleIp = "127.0.0.1";

    [JsonProperty("MultiVehicleMasterIp")]
    private string LegacyMasterIpSetter
    {
        set
        {
            if (string.IsNullOrEmpty(value) || value == "/") return;
            if (MultiVehicleMasterEndpoint == "/")
                MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008";
        }
    }

    [FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
    [FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f;
    [FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f;
    [FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f;
    [FieldMember(desc = "多车联动：X补偿阈值(mm)")] public float MultiVehiclePosBiasXThreshold = 50f;
    [FieldMember(desc = "多车联动：Y补偿阈值(mm)")] public float MultiVehiclePosBiasYThreshold = 50f;
    [FieldMember(desc = "多车联动：Th补偿阈值(deg)")] public float MultiVehiclePosBiasThThreshold = 5f;

    [FieldMember(desc = "多车联动：互识别 X补偿系数")] public float MultiVehicleDetectBiasXFac = 0.5f;
    [FieldMember(desc = "多车联动：互识别 Y补偿系数")] public float MultiVehicleDetectBiasYFac = 0.5f;
    [FieldMember(desc = "多车联动：互识别 Th补偿系数")] public float MultiVehicleDetectBiasThFac = 0.5f;
    [FieldMember(desc = "多车联动：互识别 X补偿阈值(mm)")] public float MultiVehicleDetectBiasXThreshold = 50f;
    [FieldMember(desc = "多车联动：互识别 Y补偿阈值(mm)")] public float MultiVehicleDetectBiasYThreshold = 50f;
    [FieldMember(desc = "多车联动：互识别 Th补偿阈值(deg)")] public float MultiVehicleDetectBiasThThreshold = 5f;

    // 原地旋转(mode2)闭环纠偏(PI)：把"本车应移动到的位置(dx,dy,mm)/应转角(dth,deg)"作为误差，
    // 用 PI 控制器换算成车体系修正速度叠加到绕队心旋转上。纯 P 对抗恒定横向滑移扰动有稳态残差，
    // 加积分项把稳态误差拉到 0；积分带限幅(抗 windup)，总输出限幅在 Max 内防过冲/振荡。
    // Fac=比例增益(mm/s per mm、deg/s per deg)；IFac=积分增益(mm/s per mm·s、deg/s per deg·s)；Max=总输出上限。
    [FieldMember(desc = "原地旋转纠偏：平移比例增益P(mm/s per mm)")] public float MultiVehicleRotateCompXyFac = 1.2f;
    [FieldMember(desc = "原地旋转纠偏：平移积分增益I(mm/s per mm·s)")] public float MultiVehicleRotateCompXyIFac = 0.8f;
    [FieldMember(desc = "原地旋转纠偏：平移速度上限(mm/s)")] public float MultiVehicleRotateCompXyMax = 150f;
    [FieldMember(desc = "原地旋转纠偏：转向比例增益P(deg/s per deg)")] public float MultiVehicleRotateCompThFac = 0.8f;
    [FieldMember(desc = "原地旋转纠偏：转向积分增益I(deg/s per deg·s)")] public float MultiVehicleRotateCompThIFac = 0.8f;
    [FieldMember(desc = "原地旋转纠偏：转向速度上限(deg/s)")] public float MultiVehicleRotateCompThMax = 15f;
    // 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分，
    // 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。
    [FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;

    [FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
    [FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;

    [FieldMember(desc = "Playground WebAPI 基地址")]
    public string PlaygroundWebApiUrl = "http://localhost:18090";

    [FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
    public string PlaygroundRobotName = "agv_multi_1";

    [FieldMember(desc = "Playground 邻车名称（仅主车用于原地旋转位姿诊断）")]
    public string PlaygroundNeighborRobotName = "agv_multi_2";

    [FieldMember(desc = "WebAPI 平移测试：平移距离(mm)")]
    public float WebApiTranslateMm = 100f;

    [FieldMember(desc = "WebAPI 旋转测试：旋转角度(deg)")]
    public float WebApiRotateDeg = 5f;

    [FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")]
    public float InPlaceRotateTargetWorldDeg = 90f;

    [FieldMember(desc = "原地旋转：旋转角速度(deg/s)")]
    public float InPlaceRotateSpeed = 30f;

    [FieldMember(desc = "原地旋转：到位角度精度(deg)")]
    public float InPlaceRotateArriveDeg = 1f;

    [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
    public float InPlaceRotateWheelAlignDeg = 2f;

    // ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
    [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
    public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";

    [FieldMember(desc = "2腿检测：初始猜测X(mm, 车体坐标系)")]
    public float TwoLegGuessX = 2000f;

    [FieldMember(desc = "2腿检测：两腿间距(mm)")]
    public float TwoLegWidth = 800f;

    [FieldMember(desc = "2腿检测：两腿间距允许误差(mm)")]
    public float TwoLegWidthErr = 100f;

    [FieldMember(desc = "2腿检测：聚类点间距(mm)")]
    public float TwoLegBlobDist = 100f;

    [FieldMember(desc = "2腿检测：聚类尺寸(mm)")]
    public float TwoLegBlobSize = 200f;

    [FieldMember(desc = "2腿检测：聚类最小点数")]
    public int TwoLegBlobPtCount = 5;

    [FieldMember(desc = "2腿检测：聚类 padding")]
    public int TwoLegPadding = 5;

    [FieldMember(desc = "2腿检测：腿柱搜索范围")]
    public int TwoLegPillarFindingScope = 20;

    [FieldMember(desc = "2腿检测：方向符号(±1)")]
    public int TwoLegSgnDir = 1;

    [FieldMember(desc = "2腿检测：中心X偏移(mm)")]
    public float TwoLegCenterChangeX = 0f;

    [FieldMember(desc = "2腿检测：ROI滤波框长(mm)")]
    public float TwoLegFilterLength = 1800f;

    [FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")]
    public float TwoLegFilterWidth = 600f;

    #region
    [FieldMember(desc = "轮胎识别:识别框长")] public float TireFilterLength = 1800f;
    [FieldMember(desc = "轮胎识别:识别框宽")] public float TireFilterWidth = 600f;
    [FieldMember(desc = "轮胎识别:轮胎间距")] public float TireTwoLegWidth = 800f;
    [FieldMember(desc = "轮胎识别:轮胎识别允许误差")] public float TireTwoLegWidthErr = 100f;
    [FieldMember(desc = "轮胎识别:轮胎聚类最小点云数")] public int TireTwoLegBlobPtCount = 15;

    [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobDist = 100f;
    [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobSize = 200f;
    [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontPadding = 5;
    [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegPillarFindingScope = 20;
    [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegSgnDir = 1;
    [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegCenterChangeX = 0;
    [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobDist = 100f;
    [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobSize = 200f;
    [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackPadding = 5;
    [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegPillarFindingScope = 20;
    [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegSgnDir = 1;
    [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegCenterChangeX = 0;

    [FieldMember(desc = "抱夹控制pid:Kp")] public float ClampControlKp = 0.1f;
    [FieldMember(desc = "抱夹控制pid:Ki")] public float ClampControlKi = 0f;
    [FieldMember(desc = "抱夹控制pid:Kd")] public float ClampControlKd = 0f;
    [FieldMember(desc = "抱夹控制pid:MaxI")] public float ClampControlMaxI = 0f;
    [FieldMember(desc = "抱夹控制pid:Acc")] public float ClampControlSpeedAcc = 1f;
    [FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
    [FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
    [FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;

    [FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f;
    [FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f;
    [FieldMember(desc = "直线行走Kp")] public float LineTrackKp = 0.2f;
    [FieldMember(desc = "直线行走Ki")] public float LineTrackKi = 0f;
    [FieldMember(desc = "直线行走Kd")] public float LineTrackKd = 0f;
    [FieldMember(desc = "直线行走DeadZone")] public float LineTrackDeadZone = 50f;

    [FieldMember(desc = "轮胎跟踪:切换至盲走距离")] public float TireFollowingWalkBlindSwitchingDistance = 1200f;
    [FieldMember(desc = "轮胎跟踪:识别第一对轮胎的初始距离")] public float TireFollowingStage1GuessX = 2000f;
    [FieldMember(desc = "轮胎跟踪:识别第二对轮胎的初始距离")] public float TireFollowingStage2GuessX = 2475f;
    [FieldMember(desc = "轮胎跟踪:盲走停止距离")] public float TireFollowingWalkBlindFinishDistance = 10f;
    [FieldMember(desc = "轮胎跟踪:减速距离")] public float TireFollowingSlowDistance = 200f;
    [FieldMember(desc = "轮胎跟踪:最大速度")] public float TireFollowingMaxSpeed = 0.2f;
    [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移X")] public float TireFollowingFrontLidarPathTransformationX = 253f;
    [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Y")] public float TireFollowingFrontLidarPathTransformationY = 13f;
    [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Th")] public float TireFollowingFrontLidarWalkBlindTh = -1f;
    [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移X")] public float TireFollowingBackLidarPathTransformationX = 148f;
    [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Y")] public float TireFollowingBackLidarPathTransformationY = 2f;
    [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Th")] public float TireFollowingBackLidarWalkBlindTh = 0f;
    [FieldMember(desc = "轮胎跟踪:离车时后雷达识别路径偏移X")] public float TireFollowingLeaveCarBackLidarPathTransformationX = 1500f;
    [FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f;

    [FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1;
    [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
    #endregion
}