using PA_DronePack;
using System;
using System.Collections;
using System.Collections.Generic;
using Unity.VisualScripting;
using UnityEngine;

public class UAVController : MonoBehaviour
{
    [Tooltip("sets the drone's max forward speed")]
    public float forwardSpeed = 7f;

    [Tooltip("sets the drone's max backward speed")]
    public float backwardSpeed = 5f;

    [Tooltip("sets the drone's max left strafe speed")]
    public float rightSpeed = 5f;

    [Tooltip("sets the drone's max right strafe speed")]
    public float leftSpeed = 5f;

    [Tooltip("sets the drone's max rise speed")]
    public float riseSpeed = 5f;

    [Tooltip("sets the drone's max lower speed")]
    public float lowerSpeed = 5f;

    [Tooltip("how fast the drone rotates")]
    public float turnSensitivty = 2f;

    [Tooltip("how eaisly the drone is affected by outside forces")]
    public float stability = 0.1f;//无人机稳定性

    [Tooltip("states whether or not the drone active on start")]
    public bool motorOn = true;//是否启动

    [Tooltip("单无人机模式，可控制单旋翼动力输出")]
    public bool mode_fly_by_propeller = true;

    [Tooltip("输入速度方向控制无人机")]
    public bool mode_fly_by_direction = false;

    [Tooltip("飞行方向、速度")]
    public Vector3 fly_direction;

    [Tooltip("无人机旋翼速度")]
    public float[] single_propeller_speed = new float[4];

    [Tooltip("无人机旋翼速度")]
    public float[] propeller_speed = new float[4];

    [Tooltip("makes the drone move relative to an external compass")]
    public bool headless;//相对外部指南针运动

    [Tooltip("the external compass used to control the drone's flight direction")]
    public Transform compass; //用于控制无人机飞行方向的外部罗盘

    [Tooltip("assign drone's propellers to this array")]
    [HideInInspector]
    public List<GameObject> propellers;//无人机螺旋桨

    [Tooltip("displays the drone's distance from the ground")]
    [HideInInspector]
    public float groundDistance = float.PositiveInfinity;//显示无人机距地高度

    [Tooltip("displays the drones distance from being upright")]
    [HideInInspector]
    public float uprightAngleDistance;//

    [Tooltip("displays the current propeller speed")]
    [HideInInspector]
    public float calPropSpeed;//显示当前螺旋桨速度

    [Tooltip("set propellers max spin speed")]
    [HideInInspector]
    public float propSpinSpeed = 50f;//设置螺旋桨最大旋转速度

    [Tooltip("how fast the propellers slow down")]
    [HideInInspector]
    [Range(0f, 1f)]
    public float propStopSpeed = 1f;//螺旋桨减速速度

    [Tooltip("the spark particle/object spawned on a collision")]
    [HideInInspector]
    public GameObject sparkPrefab;//碰撞火星预制体

    [Tooltip("audio clip played during flight")]
    [HideInInspector]
    public AudioSource flyingSound;//无人机飞行声音

    [Tooltip("audio clip played on collision")]
    [HideInInspector]
    public AudioSource sparkSound;//火星声音

    [Tooltip("displays the collision force of the last impact")]
    [HideInInspector]
    public float collisionMagnitude;//显示碰撞力

    [Tooltip("displays the current force lifting up the drone")]
    [HideInInspector]
    public float liftForce;//显示当前无人机升力

    [Tooltip("displays the current force driving the drone")]
    [HideInInspector]
    public float driveForce;//当前无人机前进驱动力

    [Tooltip("displays the current force strafing the drone")]
    [HideInInspector]
    public float strafeForce;//当前无人机后退驱动力

    [Tooltip("displays the current force turning the drone")]
    [HideInInspector]
    public float turnForce;//转动力

    [Tooltip("how fast the drone speeds up")]
    public float acceleration = 0.5f;//加速度

    [Tooltip("how fast the drone slows down")]
    public float deceleration = 0.2f;//减速度

    [Tooltip("the transform/location used to tilt the drone forward")]
    [HideInInspector]
    public Transform frontTilt;//用于使无人机向前倾斜的变换/位置

    [Tooltip("the transform/location used to tilt the drone backward")]
    [HideInInspector]
    public Transform backTilt;//用于使无人机向后倾斜的变换/位置

    [Tooltip("the transform/location used to tilt the drone right")]
    [HideInInspector]
    public Transform rightTilt;//用于使无人机向右倾斜的变换/位置

    [Tooltip("the transform/location used to tilt the drone left")]
    [HideInInspector]
    public Transform leftTilt;//用于使无人机向左倾斜的变换/位置

    [Tooltip("是否开启自动碰撞预警")]
    public bool autoStopBeforeCollision { get; set; }

    [Tooltip("是否自动控制飞行姿态")]
    public bool auto_AttitudeControl = true;

    [Tooltip("set whether or not the drone falls after a large impact")]
    [HideInInspector]
    public bool fallAfterCollision = true;//设置无人机在大撞击后是否坠落

    [Tooltip("sets the min. collision force used to drop the drone")]
    [HideInInspector]
    public float fallMinimumForce = 6f;//设置撞下无人机的最小力

    [Tooltip("sets the min. collision force used to create a spark")]
    [HideInInspector]
    public float sparkMinimumForce = 1f;//产生火花的最小力

    [Tooltip("displays drone's starting position")]
    [HideInInspector]
    public Vector3 startPosition;

    [Tooltip("displays the drone's rotational position")]
    [HideInInspector]
    public Quaternion startRotation;

    [Tooltip("虚拟飞行速度")]
    public float VirtualFlightSpeed;

    public Rigidbody rigidBody;
    public Rigidbody attitude_rigidBody;

    /// <summary>
    /// 加速度
    /// </summary>
    public Vector3 rigid_acceleration;

    /// <summary>
    /// 模拟飞行速率
    /// </summary>
    public float simulated_flight_speed;
    /// <summary>
    /// 模拟速度飞行方向
    /// </summary>
    public Vector3 simulated_flight_speed_direction;

    public delegate void OnCollisionWarning(string _collision);

    public OnCollisionWarning on_collision_warning;

    private RaycastHit hit;

    private Collider coll;

    /// <summary>
    /// 前后输入
    /// </summary>
    private float driveInput;

    /// <summary>
    /// 左右输入
    /// </summary>
    private float strafeInput;

    /// <summary>
    /// 上下输入
    /// </summary>
    private float liftInput;

    private float _drag;

    private float _angularDrag;

    private bool _gravity;

    private Vector3 position_Old;

    private Vector3 position_Now;

    public float horizontal_speed;

    public float vertical_speed;

    private long time_old;

    private long time_now;

    private void Awake()
    {
        coll = GetComponent<Collider>();
        rigidBody = GetComponent<Rigidbody>();
        attitude_rigidBody = transform.GetChild(0).GetComponent<Rigidbody>();
        startPosition = base.transform.position;
        startRotation = base.transform.rotation;
        _gravity = rigidBody.useGravity;
        _drag = rigidBody.drag;
        _angularDrag = rigidBody.angularDrag;

        if (!(bool)flyingSound) flyingSound = GetComponent<AudioSource>();
        if (!(bool)sparkSound) sparkSound = GetComponent<AudioSource>();
    }

    private void Start()
    {
        MeshRenderer component = GetComponent<MeshRenderer>();
        if ((bool)component)
        {
            GameObject.Destroy(component);
        }

        Transform[] componentsInChildren = base.transform.GetComponentsInChildren<Transform>(includeInactive: true);
        for (int i = 0; i < componentsInChildren.Length; i++)
        {
            if (!(componentsInChildren[i] == base.transform) && (!componentsInChildren[i].GetComponent<UAVObjectID>() || componentsInChildren[i].GetComponent<UAVObjectID>().id != "uQ3mR7quTHTw2aAy"))
            {
                base.enabled = false;
                base.hideFlags = HideFlags.NotEditable;
                Debug.LogError("ERROR: Editing " + base.name + "'s prefab is NOT allowed!\n", base.gameObject);
                Debug.LogWarning("Disabling PA_DroneController script...\n<color=#0057af>(Unlock the Full version of the drone pack to customize drones!)</color>", base.gameObject);
                return;
            }
        }

        if (headless && !compass)
        {
            Debug.LogError("no headless compassed assinged! please asign a compass in order to use headless mode!");
            headless = false;
        }
    }

    RaycastHit[] raycasts = new RaycastHit[5];//预警碰撞对象
    string msg;
    int cou;//预警碰撞数量
    private void Update()
    {
        //计算高度
        uprightAngleDistance = (1f - base.transform.up.y) * 0.5f;
        uprightAngleDistance = (((double)uprightAngleDistance < 0.001) ? 0f : uprightAngleDistance);
        if (Physics.Raycast(base.transform.position, Vector3.down, out hit, float.PositiveInfinity))
        {
            groundDistance = hit.distance;
        }

        //螺旋桨转速控制
        if (mode_fly_by_propeller)
        {
            propeller_speed[0] = (single_propeller_speed[0] != 0 ? Mathf.Clamp(single_propeller_speed[0] * Time.deltaTime, 0, 60) : (propeller_speed[0] * (1f - propStopSpeed / 2f)));
            propeller_speed[1] = (single_propeller_speed[1] != 0 ? Mathf.Clamp(single_propeller_speed[1] * Time.deltaTime, 0, 60) : (propeller_speed[1] * (1f - propStopSpeed / 2f)));
            propeller_speed[2] = (single_propeller_speed[2] != 0 ? Mathf.Clamp(single_propeller_speed[2] * Time.deltaTime, 0, 60) : (propeller_speed[2] * (1f - propStopSpeed / 2f)));
            propeller_speed[3] = (single_propeller_speed[3] != 0 ? Mathf.Clamp(single_propeller_speed[3] * Time.deltaTime, 0, 60) : (propeller_speed[3] * (1f - propStopSpeed / 2f)));

            propellers[0].transform.Rotate(0f, 0f, -propeller_speed[0]);
            propellers[1].transform.Rotate(0f, 0f, propeller_speed[1]);
            propellers[2].transform.Rotate(0f, 0f, propeller_speed[2]);
            propellers[3].transform.Rotate(0f, 0f, -propeller_speed[3]);
        }
        else
        {

            calPropSpeed = (motorOn ? propSpinSpeed : (calPropSpeed * (1f - propStopSpeed / 2f)));
            propellers[0].transform.Rotate(0f, 0f, -calPropSpeed);
            propellers[1].transform.Rotate(0f, 0f, calPropSpeed);
            propellers[2].transform.Rotate(0f, 0f, calPropSpeed);
            propellers[3].transform.Rotate(0f, 0f, -calPropSpeed);
        }

        //是否有声音文件
        if ((bool)flyingSound)
        {
            flyingSound.volume = calPropSpeed / propSpinSpeed;
            flyingSound.pitch = 1f + liftForce * 0.02f;
        }
    }

    private void FixedUpdate()
    {

        position_Old = position_Now;
        time_old = time_now;
        if (motorOn)
        {

            rigidBody.useGravity = false;
            rigidBody.drag = 0f;
            rigidBody.angularDrag = 0f;

            if (mode_fly_by_direction)
            {
                //rigidBody.velocity = fly_direction;
                //transform.

            }
            else
            {
                if (headless)
                {
                    if (!compass)
                    {
                        Debug.LogError("no headless compassed assinged! please asign a compass in order to use headless mode!");
                        headless = false;
                    }

                    if (groundDistance > 0.2f && (driveInput != 0f || strafeInput != 0f))
                    {
                        rigidBody.AddForceAtPosition(Vector3.down, rigidBody.position + rigidBody.velocity.normalized * 0.5f, ForceMode.Acceleration);
                    }

                    Vector3 direction = compass.InverseTransformDirection(rigidBody.velocity);
                    direction.z = ((driveInput != 0f) ? Mathf.Lerp(direction.z, driveInput, acceleration * 0.3f) : Mathf.Lerp(direction.z, driveInput, deceleration * 0.2f));
                    driveForce = ((Mathf.Abs(direction.z) > 0.01f) ? direction.z : 0f);
                    direction.x = ((strafeInput != 0f) ? Mathf.Lerp(direction.x, strafeInput, acceleration * 0.3f) : Mathf.Lerp(direction.x, strafeInput, deceleration * 0.2f));
                    strafeForce = ((Mathf.Abs(direction.x) > 0.01f) ? direction.x : 0f);
                    rigidBody.velocity = compass.TransformDirection(direction);
                }
                else
                {
                    //姿态控制,影响定点飞行控制
                    if (auto_AttitudeControl)
                    {
                        if (driveInput > 0f)
                        {
                            //rigidBody.AddForceAtPosition(Vector3.down * (Mathf.Abs(driveInput) * 0.3f), frontTilt.position, ForceMode.Acceleration);
                            rigidBody.AddForceAtPosition(-frontTilt.up * (Mathf.Abs(driveInput) * 0.3f), frontTilt.position, ForceMode.Acceleration);
                        }

                        if (driveInput < 0f)
                        {
                            //rigidBody.AddForceAtPosition(Vector3.down * (Mathf.Abs(driveInput) * 0.3f), backTilt.position, ForceMode.Acceleration);
                            rigidBody.AddForceAtPosition(-frontTilt.up * (Mathf.Abs(driveInput) * 0.3f), backTilt.position, ForceMode.Acceleration);
                        }

                        if (strafeInput > 0f)
                        {
                            //rigidBody.AddForceAtPosition(Vector3.down * (Mathf.Abs(strafeInput) * 0.3f), rightTilt.position, ForceMode.Acceleration);
                            rigidBody.AddForceAtPosition(-frontTilt.up * (Mathf.Abs(strafeInput) * 0.3f), rightTilt.position, ForceMode.Acceleration);
                        }

                        if (strafeInput < 0f)
                        {
                            //rigidBody.AddForceAtPosition(Vector3.down * (Mathf.Abs(strafeInput) * 0.3f), leftTilt.position, ForceMode.Acceleration);
                            rigidBody.AddForceAtPosition(-frontTilt.up * (Mathf.Abs(strafeInput) * 0.3f), leftTilt.position, ForceMode.Acceleration);
                        }
                    }

                    //水平移动
                    Vector3 direction2 = base.transform.InverseTransformDirection(rigidBody.velocity);
                    direction2.z = ((driveInput != 0f) ? Mathf.Lerp(direction2.z, driveInput, acceleration * 0.3f) : Mathf.Lerp(direction2.z, driveInput, deceleration * 0.2f));
                    driveForce = ((Mathf.Abs(direction2.z) > 0.01f) ? direction2.z : 0f);
                    direction2.x = ((strafeInput != 0f) ? Mathf.Lerp(direction2.x, strafeInput, acceleration * 0.3f) : Mathf.Lerp(direction2.x, strafeInput, deceleration * 0.2f));
                    strafeForce = ((Mathf.Abs(direction2.x) > 0.01f) ? direction2.x : 0f);
                    rigidBody.velocity = base.transform.TransformDirection(direction2);
                }

                //垂直移动
                liftForce = ((liftInput != 0f) ? Mathf.Lerp(liftForce, liftInput, acceleration * 0.3f) : Mathf.Lerp(liftForce, liftInput, deceleration * 0.3f));
                liftForce = ((Mathf.Abs(liftForce) > 0.01f) ? liftForce : 0f);
                rigidBody.velocity = new Vector3(rigidBody.velocity.x, liftForce, rigidBody.velocity.z);

                //旋转
                rigidBody.angularVelocity *= 1f - Mathf.Clamp(InputMagnitude(), 0.2f, 1f) * stability;
                Quaternion quaternion = Quaternion.FromToRotation(base.transform.up, Vector3.up);
                rigidBody.AddTorque(new Vector3(quaternion.x, 0f, quaternion.z) * 100f, ForceMode.Acceleration);
                rigidBody.angularVelocity = new Vector3(rigidBody.angularVelocity.x, turnForce, rigidBody.angularVelocity.z);
            }
        }
        else
        {
            rigidBody.useGravity = _gravity;
            rigidBody.drag = _drag;
            rigidBody.angularDrag = _angularDrag;
        }

        #region 碰撞预警及自动停止
        if (autoStopBeforeCollision)
        {
            if (simulated_flight_speed != 0)
            {
                cou = Physics.RaycastNonAlloc(transform.position, simulated_flight_speed_direction, raycasts, 10, UAVManager.Instance.ColliderLayer);
                Debug.DrawRay(transform.position, simulated_flight_speed_direction, Color.blue);

                AutoStop();
            }
            else if (rigidBody.velocity != Vector3.zero)
            {
                cou = Physics.RaycastNonAlloc(transform.position, rigidBody.velocity, raycasts, 10, UAVManager.Instance.ColliderLayer);
                Debug.DrawRay(transform.position, rigidBody.velocity, Color.red);

                AutoStop();
            }


        }
        #endregion

        position_Now = this.transform.position;
        time_now = DateTimeToLongTimeStamp(DateTime.Now);
        Horizontal_speed();
        Vertical_speed();
    }

    void AutoStop()
    {
        if (cou > 0)
        {
            if (raycasts != null && raycasts.Length > 0)
            {
                for (int i = 0; i < raycasts.Length; i++)
                {
                    var hitdic = raycasts[i].point - transform.position;
                    if (raycasts[i].transform != null)
                    {
                        msg = string.Format("碰撞预警：{0}，方向：{1}，当前速度：{2}", raycasts[i].transform.name, hitdic, rigidBody.velocity);
                        Debug.Log(message: msg);
                        Debug.Log(simulated_flight_speed_direction);
                        Debug.Log(rigidBody.velocity);
                        on_collision_warning?.Invoke(msg);
                        if (raycasts[i].distance < 2)
                        {
                            Debug.Log("警告！即将发生碰撞，系统保护机制介入！");
                            UAVManager.Instance.UpdateAllSpeed(new Vector4(5, 5, 5, 5));
                        }
                    }
                }
            }
        }
    }

    //private void OnGUI()
    //{
    //    GUIStyle style = new GUIStyle();
    //    style.fontSize = 24;
    //    GUILayout.Label(msg, style);
    //    GUILayout.Label(string.Format("当前位置：{0}；当前速度：{1}；速度值{2}", transform.position, rigidBody.velocity, rigidBody.velocity.magnitude), style);
    //}

    private void OnCollisionEnter(Collision newObject)
    {
        collisionMagnitude = newObject.relativeVelocity.magnitude;

        //非动力运行情况
        var newUav = newObject.transform.GetComponent<UAVController>();
        if (Mathf.Abs(collisionMagnitude - 0.01f) < 0.01f)
        {

        }
        if (collisionMagnitude > sparkMinimumForce)
        {
            SpawnSparkPrefab(newObject.contacts[0].point);
            if ((bool)sparkSound)
            {
                sparkSound.pitch = collisionMagnitude * 0.1f;
                sparkSound.PlayOneShot(sparkSound.clip, collisionMagnitude * 0.05f);
            }
        }

        if (collisionMagnitude > fallMinimumForce && fallAfterCollision)
        {
            motorOn = false;
        }
    }
    private void OnCollisionStay(Collision newObject)
    {
        if (groundDistance < coll.bounds.extents.y + 0.15f)
        {
            liftForce = Mathf.Clamp(liftForce, 0f, float.PositiveInfinity);
        }
    }

    /// <summary>
    /// 定点飞行模式
    /// </summary>
    public bool is_fixed_mode;

    public void ToggleTargetMode()
    {
        is_fixed_mode = true;
    }

    /// <summary>
    /// 单旋翼控制模式
    /// </summary>
    /// <param name="index">旋翼编号</param>
    /// <param name="speed"></param>
    public void SingleDrone(int index, float speed)
    {
        single_propeller_speed[index] = speed * 500;

        if (mode_fly_by_propeller)
        {
            float force = speed * 0.025f;

            if (force != 0)
            {
                switch (index)
                {
                    case 0:
                        StrafeInput(force);
                        DriveInput(-force);
                        break;
                    case 1:
                        StrafeInput(-force);
                        DriveInput(-force);
                        break;
                    case 2:
                        StrafeInput(force);
                        DriveInput(force);
                        break;
                    case 3:
                        StrafeInput(-force);
                        DriveInput(force);
                        break;
                    default:
                        break;
                }
            }
        }
    }

    /// <summary>
    /// 引擎启停
    /// </summary>
    /// <param name="toggle"></param>
    public void ToggleMotor(int toggle = -1)
    {
        if (toggle == -1)
            motorOn = !motorOn;
        else
            motorOn = toggle == 1;
    }

    public void ToggleHeadless()
    {
        headless = !headless;
    }

    /// <summary>
    /// 前后移动输入
    /// </summary>
    /// <param name="input"></param>
    public void DriveInput(float input)
    {
        if (input > 0f)
        {
            driveInput = input * forwardSpeed;
        }
        else if (input < 0f)
        {
            driveInput = input * backwardSpeed;
        }
        else
        {
            driveInput = 0f;
        }
    }

    /// <summary>
    /// 左右移动输入
    /// </summary>
    /// <param name="input"></param>
    public void StrafeInput(float input)
    {
        if (input > 0f)
        {
            strafeInput = input * rightSpeed;
        }
        else if (input < 0f)
        {
            strafeInput = input * leftSpeed;
        }
        else
        {
            strafeInput = 0f;
        }
    }

    /// <summary>
    /// 上下移动输入
    /// </summary>
    /// <param name="input"></param>
    public void LiftInput(float input)
    {
        if (input > 0f)
        {
            liftInput = input * riseSpeed;
            motorOn = true;
        }
        else if (input < 0f)
        {
            liftInput = input * lowerSpeed;
        }
        else
        {
            liftInput = 0f;
        }
    }

    /// <summary>
    /// 旋转输入
    /// </summary>
    /// <param name="input"></param>
    public void TurnInput(float input)
    {
        turnForce = input * turnSensitivty;
    }

    public void ResetDronePosition()
    {
        rigidBody.position = startPosition;
        rigidBody.rotation = startRotation;
        rigidBody.velocity = Vector3.zero;
    }

    public void SpawnSparkPrefab(Vector3 position)
    {
        GameObject obj = GameObject.Instantiate(sparkPrefab, position, Quaternion.identity);
        ParticleSystem.MainModule main = obj.GetComponent<ParticleSystem>().main;
        GameObject.Destroy(obj, main.duration + main.startLifetime.constantMax);
    }

    public void AdjustLift(float value)
    {
        riseSpeed = value;
        lowerSpeed = value;
    }

    public void AdjustSpeed(float value)
    {
        forwardSpeed = value;
        backwardSpeed = value;
    }

    public void AdjustStrafe(float value)
    {
        rightSpeed = value;
        leftSpeed = value;
    }

    public void AdjustTurn(float value)
    {
        turnSensitivty = value;
    }

    public void AdjustAccel(float value)
    {
        acceleration = value;
    }

    public void AdjustDecel(float value)
    {
        deceleration = value;
    }

    public void AdjustStable(float value)
    {
        stability = value;
    }

    public void ToggleFall(bool state)
    {
        fallAfterCollision = !fallAfterCollision;
    }

    public void ChangeFlightAudio(AudioClip newClip)
    {
        flyingSound.clip = newClip;
        flyingSound.enabled = false;
        flyingSound.enabled = true;
    }

    public void ChangeImpactAudio(AudioClip newClip)
    {
        sparkSound.clip = newClip;
        sparkSound.enabled = false;
        sparkSound.enabled = true;
    }

    private float InputMagnitude()
    {
        return (Mathf.Abs(driveInput) + Mathf.Abs(strafeInput) + Mathf.Abs(liftInput)) / 3f;
    }

    public void Horizontal_speed()
    {
        float M = float.Parse((time_now - time_old).ToString());
        horizontal_speed = Mathf.Sqrt((position_Now.x - position_Old.x) * (position_Now.x - position_Old.x) + (position_Now.z - position_Old.z) * (position_Now.z - position_Old.z))/ (M/1000f);
    }

    public void Vertical_speed()
    {
        float M = float.Parse((time_now - time_old).ToString());
        vertical_speed =(position_Now.y - position_Old.y) / (M / 1000f);
    }


    public static long DateTimeToLongTimeStamp(DateTime dateTime)
    {
        return (long)(dateTime.ToUniversalTime() - new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc)).TotalMilliseconds;
    }
}