using EVP;
using UnityEngine;

namespace Script
{
    using ModbusManager;
    using System;
    using System.Threading.Tasks;
    using Unity.VisualScripting;
    using UnityEditor.PackageManager;
    using UnityEngine;
    using static UnityEditor.LightingExplorerTableColumn;

    public class CarGearControl : MonoBehaviour
    {
        private VehicleController vehicleController;

        private VehicleStandardInput vehicleStandardInput;
        public int currentGear = 0; // 当前挡位
        public float[] gearSpeeds = { 0f, 20f, 40f, 60f }; // 每个挡位的最大速度

        public float xx;
        float steerInput, throttleInput, brakeInput, handbrakeInput;

        private ModbusTcpClient client;
        private CarStatusData carData;

        float minValue = -300f; // 范围的最小值
        float maxValue = 300; // 范围的最大值

        public CarInfoManager carInfoManager;
        void Start()
        {
            vehicleController = GetComponent<VehicleController>();
            vehicleStandardInput = GetComponent<VehicleStandardInput>();

            StartModbus();
            ModBusQueue();
            
            
        }

        async Task StartModbus()
        {
            client = new ModbusTcpClient();
            await client.ConnectToServer();
            while (true)
            {
                await client.SendModbusRequest();

                await Task.Delay(TimeSpan.FromSeconds(.1));
            }
        }

        void OnDestroy()
        {
            client.CloseConnection();
        }

        async Task ModBusQueue()
        {
            while (true)
            {
                if (client.modbusQueue.Count > 0)
                {
                    carData = client.modbusQueue.Dequeue();
                    // Debug.Log(carData.SteeringWheelAngle);
                    //方向盘
                    vehicleController.steerInput = NormalizeValue(carData.SteeringWheelAngle, minValue, maxValue);
                    //手刹
                    // vehicleController.throttleInput = carData.HandbrakeStatus == 0 ? 1 : 0;
                    //挡位 00空挡，1前进档，2倒挡，3为P档
                    switch (carData.GearPosition)
                    {
                        case 0:
                            break;
                        case 1://1前进档
                            if (carData.HandbrakeStatus == 0)
                            {
                                if (carData.AcceleratorPedalPosition > 0)
                                {
                                    vehicleController.throttleInput = 1;
                                }
                                else
                                {
                                    vehicleController.throttleInput = 0;
                                }
                            }
                            else
                            {
                                vehicleController.throttleInput = 0;
                            }

                            break;
                        case 2://2倒挡
                            if (carData.HandbrakeStatus == 0)
                            {
                                if (carData.AcceleratorPedalPosition > 0)
                                {
                                    vehicleController.throttleInput = -0.5f;
                                }
                                else
                                {
                                    vehicleController.throttleInput = 0;
                                }
                            }
                            else
                            {
                                vehicleController.throttleInput = 0;
                            }
                            break;
                        case 3:
                            break;
                    }
                    
                    //转向灯 00是未开转向灯，1是左转向灯，2是右转向灯
                    switch (carData.TurnSignalStatus)
                    {
                        case 0:
                            carInfoManager.StopBlinking();
                            break;
                        case 1:
                            carInfoManager.LeftStartBlinking();
                            break;
                        case 2:
                            carInfoManager.RightStartBlinking();
                            break;
                    }
                    
                }

                await Task.Delay(TimeSpan.FromSeconds(.1));
            }
        }


        void Update()
        {
            // 检测并更新挡位
            CheckAndUpdateGear();

            // 根据当前挡位设置车辆速度
            SetVehicleSpeed(gearSpeeds[currentGear]);

            //vehicleController.steerInput = 0;

            vehicleController.brakeInput = 0;
            vehicleController.handbrakeInput = 0;
            //Debug.Log($"{steerInput}--{throttleInput}--{brakeInput}--{handbrakeInput}");

            // vehicleStandardInput.SetThrottleValue(xx);

            // SetSpeedBasedOnValue(1);
        }

        void CheckAndUpdateGear()
        {
            if (Input.GetKeyDown(KeyCode.Alpha1)) currentGear = 1;
            if (Input.GetKeyDown(KeyCode.Alpha2)) currentGear = 2;
            if (Input.GetKeyDown(KeyCode.Alpha3)) currentGear = 3;
            if (Input.GetKeyDown(KeyCode.Alpha0)) currentGear = 0; // 假设0为空挡
        }

        void SetVehicleSpeed(float maxSpeed)
        {
            // 在这里，你需要根据你的车辆控制系统来调整速度。
            // 以下是一个假设的方法，你可能需要根据EVP5的API来修改它。
            vehicleController.maxSpeedForward = maxSpeed;
            //vehicleController.throttleInput = 1;
        }

        public float NormalizeValue(float originalValue, float minValue, float maxValue)
        {
            // 确保原始值在范围内
            float clampedValue = Math.Max(minValue, Math.Min(originalValue, maxValue));

            // 计算归一化值
            float normalizedValue = (2 * (clampedValue - minValue) / (maxValue - minValue)) - 1;

            return normalizedValue;
        }
    }
}