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;
        
        double originalValue = 75.0; 
        double minValue = 0.0; // 范围的最小值
        double maxValue = 850; // 范围的最大值
        
        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(.2));
            }
        }
        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();

                }

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


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

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

            //vehicleController.steerInput = 0;
            //vehicleController.throttleInput = xx;
            //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  double NormalizeValue(double originalValue, double minValue, double maxValue)
        {
            // 确保原始值在范围内
            double clampedValue = Math.Max(minValue, Math.Min(originalValue, maxValue));


            // 计算归一化值
            double normalizedValue = (clampedValue - minValue) / (maxValue - minValue);


            return normalizedValue;
        }

    }

}