using UnityEngine;
using System;
using System.Collections;

public class ArianplaneController : MonoBehaviour {
    public float maxSustentation = 9.81F;

    public Transform centerOfMass;

    public Transform alaDerecha;
        public bool alaDerechaActiva = true;

    public Transform alaIzquierda;
        public bool alaIzquierdaActiva = true;

    public ArianplaneWheels wheels;
    public float raycastDistance = 0.3F;

    private bool isOnTheFloor = true;

    // Debug variables
        private bool _debugMode = false;

    void Start() {

    }

    // GUI shit
        void OnGUI() {
            if (this._debugMode)
                this._Debug();
        }

        private void _Debug() {
            GUI.BeginGroup(new Rect(10, 10, 200, 300));
            GUI.Box(new Rect(0, 0, 200, 300), "Debug information");
            GUI.Label(new Rect(5, 20, 200, 30), "Is on the floor: "+this.isOnTheFloor);
            GUI.EndGroup();
        }

    void Update() {
        if(Input.GetKeyDown(KeyCode.F2))
            this._debugMode = !_debugMode;

        _isOnTheFloor();
    }

    void FixedUpdate() {
        rigidbody.centerOfMass = centerOfMass.localPosition;

        if (alaDerechaActiva)
            rigidbody.AddForceAtPosition(new Vector3(0, maxSustentation/2 * rigidbody.mass, 0), alaDerecha.position);
        if (alaIzquierdaActiva)
            rigidbody.AddForceAtPosition(new Vector3(0, maxSustentation/2 * rigidbody.mass, 0), alaIzquierda.position);


    }

    private void _controls() {

    }

    private bool _isOnTheFloor() {
        bool one = false;
        bool two = false;
        bool three = false;

        Debug.DrawRay(wheels.frontLeft.position, -wheels.frontLeft.up, Color.blue, 0.01F);
        if(Physics.Raycast(wheels.frontLeft.position, -wheels.frontLeft.up, raycastDistance)) {
            one = true;
        } else {
            one = false;
        }

        Debug.DrawRay(wheels.frontRight.position, Vector3.down, Color.blue, 0.01F);
        if(Physics.Raycast(wheels.frontRight.position, Vector3.down, raycastDistance)) {
            two = true;
        } else {
            two = false;
        }

        Debug.DrawRay(wheels.back.position, Vector3.down, Color.blue, 0.01F);
        if(Physics.Raycast(wheels.back.position, Vector3.down, raycastDistance)) {
            three = true;
        } else {
            three = false;
        }

        if (one && two && three) {
            this.isOnTheFloor = true;
            return true;
        } else {
            this.isOnTheFloor = false;
            return false;
        }

    }

    /*private float elapsed;

    // Use this for initialization
    void Start () {

    }

    // Update is called once per frame
    void Update () {
        UpdateForces();
    }

    //************************************************************
    // Use PhysX rigidbody to update forces acting on a plane
    //************************************************************
    public void UpdateForces()
    {
        elapsed += Time.deltaTime;

        //************************************************************
        // Update Roll & Yaw
        //************************************************************
        float ax = 0;
        float ay = 0;


        //************************************************************
        // Accelerometer controls, cater to specific situations
        //************************************************************
        if ( FlightStick.gameObject.active )
        {

             torqueY = 0.1f * FlightStick.NormalizedX * FlightManager.Data.Handling;

             torqueX = 400 * FlightStick.NormalizedY * (1-FlightManager.Data.Stability);

           EngineThrust += 25 * ThrustStick.NormalizedY;
           if( EngineThrust <= 0 )
           {
             EngineThrust = 0;
           }
           else if ( EngineThrust >= MaxThrust )
           {
             EngineThrust = MaxThrust;
           }
        }

        //************************************************************
        //4 Major forces on Plane: Thrust, Lift, Weight, Drag
        //************************************************************
        Vector3 airSpeedVector = -rigidbody.velocity;

        float angleOfAttack = -Mathf.Deg2Rad * Vector3.Dot( rigidbody.velocity, transform.up);
        float slipAoA = -Mathf.Deg2Rad * Vector3.Dot( rigidbody.velocity, transform.right);
        float speedWeight = AirDensity * ((CurrentSpeed*CurrentSpeed)/2);
        float weightCo = Weight;

        sideSlipCoefficient = 0.001f * slipAoA * speedWeight;
        liftCoefficient = WingArea * angleOfAttack * speedWeight;

        //************************************************************
        //drag goes from x -> 3x * %
        //angle goes from -1 -> 1
        //************************************************************
        float horizonAngle = Vector3.Dot( transform.forward, Vector3.up);
        dragCoefficient = 2*AirDrag + 2 * AirDrag * horizonAngle;
        thrustCoefficient = EngineThrust;
        AoA = Vector3.Dot( transform.up, Vector3.up);


        //************************************************************
        // Pitch from lift, yaw & roll from sideslip
        //************************************************************
        rollCoEfficient = -(torqueY+ay) * RollRate * speedWeight;
        pitchCoEfficient =  (torqueX+ax) * ElevatorRate * speedWeight;
        yawCoEfficient = 0;

        pitchCoEfficient += liftCoefficient * 0.001f;
        yawCoEfficient += -sideSlipCoefficient * 2f;
        rollCoEfficient += -sideSlipCoefficient * 0.1f;

        //************************************************************
        // Calc vectors now we have coefficients
        //************************************************************
        WeightVector = Vector3.down * weightCo;


        ThrustForceVector = transform.forward * thrustCoefficient;
        LiftVector = transform.up * liftCoefficient;
        DragVector = airSpeedVector * dragCoefficient;
        SideSlip = Vector3.right * sideSlipCoefficient;

        RollTorque = Vector3.forward * rollCoEfficient;
        YawTorque = Vector3.up * yawCoEfficient;
        PitchTorque = Vector3.right * pitchCoEfficient;

        //************************************************************
        // Apply forces
        //************************************************************
        rigidbody.AddForce(ThrustForceVector, ForceMode.Force);
        rigidbody.AddForce(DragVector, ForceMode.Force);
        rigidbody.AddForce(LiftVector, ForceMode.Force);
        rigidbody.AddForce(WeightVector, ForceMode.Force);
        rigidbody.AddForce(SideSlip, ForceMode.Force);

        rigidbody.AddRelativeTorque(RollTorque);
        rigidbody.AddRelativeTorque(YawTorque);
        rigidbody.AddRelativeTorque(PitchTorque);

        //************************************************************
        // Instrumentation data & debug output
        //************************************************************
        VelocityVector = rigidbody.velocity;

        PlaneLift = liftCoefficient;
        //AoA = angleOfAttack;
        RotX = transform.rotation.eulerAngles.x;
        RotY = transform.rotation.eulerAngles.y;
        RotZ = transform.rotation.eulerAngles.z;
        torqueX = 0;
        torqueY = 0;
        CurrentSpeed = VelocityVector.magnitude;
        CurrentFuel -= Time.deltaTime * Weight/5 * EngineThrust/MaxThrust * 0.3f * FuelConsumption;

    }*/
}

[Serializable]

public class ArianplaneWheels {
    public Transform frontLeft;
    public Transform frontRight;
    public Transform back;
}