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

public class Moni : MonoBehaviour
{
    public UAVController Controller;

    public float xspeed = 50;

    [Range(0, 10)]
    public float fin0, fin1, fin2, fin3;

    // Start is called before the first frame update
    void Start()
    {

    }
    public Rigidbody rigidBody;

    public Vector3 frontForce;
    public Vector3 rightForce;
    public Vector3 backForce;
    public Vector3 leftForce;
    public float driveInputFront;
    public float driveInputRight;
    public float driveInputBack;
    public float driveInputLeft;
    public Transform frontTilt;
    public Transform rightTilt;
    public Transform backTilt;
    public Transform leftTilt;
    public ForceMode forceMode = ForceMode.Acceleration;


    public bool setall; 

    public float publicInputForce;

    [Range (0, 1)]
    public float stability = 0.1f;
    public float turnForce;

    // Update is called once per frame
    void FixedUpdate()
    {

        if (!setall)
        {

            //rigidBody.AddForceAtPosition(frontForce * (Mathf.Abs(driveInputFront) * 0.3f), frontTilt.position, forceMode);
            //rigidBody.AddForceAtPosition(rightForce * (Mathf.Abs(driveInputRight) * 0.3f), rightTilt.position, forceMode);
            //rigidBody.AddForceAtPosition(backForce * (Mathf.Abs(driveInputBack) * 0.3f), backTilt.position, forceMode);
            //rigidBody.AddForceAtPosition(leftForce * (Mathf.Abs(driveInputLeft) * 0.3f), leftTilt.position, forceMode);

            //rigidBody.AddForceAtPosition(frontTilt.forward * (Mathf.Abs(driveInputFront) * 0.3f), frontTilt.position, forceMode);
            //rigidBody.AddForceAtPosition(rightTilt.forward * (Mathf.Abs(driveInputRight) * 0.3f), rightTilt.position, forceMode);
            //rigidBody.AddForceAtPosition(backTilt.forward * (Mathf.Abs(driveInputBack) * 0.3f), backTilt.position, forceMode);
            //rigidBody.AddForceAtPosition(leftTilt.forward * (Mathf.Abs(driveInputLeft) * 0.3f), leftTilt.position, forceMode);

            rigidBody.AddForceAtPosition(frontTilt.up * (Mathf.Abs(driveInputFront) * 0.3f), frontTilt.position, forceMode);
            rigidBody.AddForceAtPosition(rightTilt.up * (Mathf.Abs(driveInputRight) * 0.3f), rightTilt.position, forceMode);
            rigidBody.AddForceAtPosition(backTilt.up * (Mathf.Abs(driveInputBack) * 0.3f), backTilt.position, forceMode);
            rigidBody.AddForceAtPosition(leftTilt.up * (Mathf.Abs(driveInputLeft) * 0.3f), leftTilt.position, forceMode);

            Quaternion quaternion = Quaternion.FromToRotation(base.transform.up, Vector3.up);
            rigidBody.AddTorque(new Vector3(quaternion.x, 0f, quaternion.z) * 100f, ForceMode.Acceleration);

            //Controller.SingleDrone(0, fin0 * xspeed);
            //Controller.SingleDrone(1, fin1 * xspeed);
            //Controller.SingleDrone(2, fin2 * xspeed);
            //Controller.SingleDrone(3, fin3 * xspeed);

            Controller.SingleDrone(0, driveInputFront * xspeed);
            Controller.SingleDrone(1, driveInputRight * xspeed);
            Controller.SingleDrone(2, driveInputBack * xspeed);
            Controller.SingleDrone(3, driveInputLeft * xspeed);
        }

        else
        {
            //rigidBody.AddForceAtPosition(frontForce * (Mathf.Abs(publicInputForce) * 0.3f), frontTilt.position, forceMode);
            //rigidBody.AddForceAtPosition(rightForce * (Mathf.Abs(publicInputForce) * 0.3f), rightTilt.position, forceMode);
            //rigidBody.AddForceAtPosition(backForce * (Mathf.Abs(publicInputForce) * 0.3f), backTilt.position, forceMode);
            //rigidBody.AddForceAtPosition(leftForce * (Mathf.Abs(publicInputForce) * 0.3f), leftTilt.position, forceMode);

            //rigidBody.AddForceAtPosition(frontTilt.forward * (Mathf.Abs(publicInputForce) * 0.3f), frontTilt.position, forceMode);
            //rigidBody.AddForceAtPosition(rightTilt.forward * (Mathf.Abs(publicInputForce) * 0.3f), rightTilt.position, forceMode);
            //rigidBody.AddForceAtPosition(backTilt.forward * (Mathf.Abs(publicInputForce) * 0.3f), backTilt.position, forceMode);
            //rigidBody.AddForceAtPosition(leftTilt.forward * (Mathf.Abs(publicInputForce) * 0.3f), leftTilt.position, forceMode);
            
            rigidBody.AddForceAtPosition(frontTilt.up * (Mathf.Abs(publicInputForce) * 0.3f), frontTilt.position, forceMode);
            rigidBody.AddForceAtPosition(rightTilt.up * (Mathf.Abs(publicInputForce) * 0.3f), rightTilt.position, forceMode);
            rigidBody.AddForceAtPosition(backTilt.up * (Mathf.Abs(publicInputForce) * 0.3f), backTilt.position, forceMode);
            rigidBody.AddForceAtPosition(leftTilt.up * (Mathf.Abs(publicInputForce) * 0.3f), leftTilt.position, forceMode);

            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);

            //Controller.SingleDrone(0, fin0 * xspeed);
            //Controller.SingleDrone(1, fin1 * xspeed);
            //Controller.SingleDrone(2, fin2 * xspeed);
            //Controller.SingleDrone(3, fin3 * xspeed);

            Controller.SingleDrone(0, publicInputForce * xspeed);
            Controller.SingleDrone(1, publicInputForce * xspeed);
            Controller.SingleDrone(2, publicInputForce * xspeed);
            Controller.SingleDrone(3, publicInputForce * xspeed);
        }
    }

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