//========================================================================================================================
// Edy Vehicle Physics - (c) Angel Garcia "Edy" - Oviedo, Spain
// http://www.edy.es/dev/vehicle-physics
//
// Terms & Conditions:
//  - Use for unlimited time, any number of projects, royalty-free.
//  - Keep the copyright notices on top of the source files.
//  - Resale or redistribute as anything except a final product to the end user (asset / library / engine / middleware / etc.) is not allowed.
//  - Put me (Angel Garcia "Edy") in your game's credits as author of the vehicle physics.
//
// Bug reports, improvements to the code, suggestions on further developments, etc are always welcome.
// Unity forum user: Edy
//========================================================================================================================
//
// CarControl
//
// Main script for controlling and configuring the vehicle. All parameters that affect the behavior & handling
// are defined here (except suspension parameters: spring, damper, distance, radius, which are configured on each wheel).
//
//========================================================================================================================
using UnityEngine;

public class CarControl : BMBSingleton<CarControl>
{

    // Objetos controlados

    public CarWheel WheelFL;
    public CarWheel WheelFR;
    public CarWheel WheelRL;
    public CarWheel WheelRR;
    public CarAntiRollBar AntiRollFront;
    public CarAntiRollBar AntiRollRear;
    public Transform CenterOfMass;

    // Curvas de fricción de las ruedas
    public CarFrictionCurve ForwardWheelFriction;
    public CarFrictionCurve SidewaysWheelFriction;
    public bool optimized = false;				// En modo optimizado los parámetros de las curvas de fricción no se pueden cambiar en tiempo real (no responden adecuadamente).

    // Parámetros de entrada

    public bool readUserInput = false;			// Reads user input itself instead of depending of externally provided input. This flag is automatically disabled when the car is controlled from a CarMain script.
    public bool reverseRequiresStop = false;	// Applicable only when readUserInput is true. If the car is controlled from a CarMain script then the global CarMain.reverseRequiresStop flag applies.
    public float steerInput = 0.0f;
    public float motorInput = 0.0f;
    public float brakeInput = 0.0f;
    public float handbrakeInput = 0.0f;
    public int gearInput = 1;

    // Parámetros de rendimiento y comportamiento

    public float steerMax = 45.0f;
    public float motorMax = 1.0f;			// Cantidad máxima de slip de aceleración antes de TC
    public float brakeMax = 1.0f;			// Cantidad máxima de slip de frenado antes de ABS

    public float autoSteerLevel = 1.0f;	// ESP
    public bool autoMotorMax = true;	// TC
    public bool autoBrakeMax = true;	// ABS
    public float antiRollLevel = 1.0f;	// Barras estabilizadoras

    public float motorPerformancePeak = 5.0f;		// m/s de aceleración máxima en modo 4x4. En tracción simple es necesario subir el motorForceFactor
    public float motorForceFactor = 1.0f;
    public float motorBalance = 0.5f;				// 0.0 = tracción delantera, 1.0 = tracción trasera, 0.5 = 4x4.
    public float brakeForceFactor = 1.5f;
    public float brakeBalance = 0.5f;				// 0.0 = todo adelante, 1.0 = todo atrás, 0.5 = mismo nivel.

    public float sidewaysDriftFriction = 0.35f;
    public float staticFrictionMax = 1500.0f;		// Control de fricción - ver CarWheel.
    public float frontSidewaysGrip = 1.0f;
    public float rearSidewaysGrip = 1.0f;

    // Parámetros de configuración

    public bool serviceMode = false;			// Si está activado se asignan los parámetros de grip tal cual. Permite hacer las pruebas de las curvas de fricción.

    public float airDrag = 3.0f * 2.2f;		// Coeficiente aerodinámico (ej. corvette 0.30) * Area frontal
    public float frictionDrag = 30.0f;	// 30 veces la resistencia del aire (así airDrag será más importante a partir de 30 m/s = 100 Km/h)

    public float rollingFrictionSlip = 0.075f;

    // MODO EXPERIMENTAL V3:
    // - Reduce la tracción a medida que se aproxima a la velocidad máxima. Alcanzando ésta la tracción es 0.

    public bool tractionV3 = false;
    public float maxSpeed = 40.0f;


    // ------------------------------------------------
    // Datos privados + telemetría

    private bool m_brakeRelease = false;
    private float m_motorSlip = 0.0f;
    private float m_brakeSlip = 0.0f;
    private float m_frontMotorSlip = 0.0f;
    private float m_frontBrakeSlip = 0.0f;
    private float m_rearMotorSlip = 0.0f;
    private float m_rearBrakeSlip = 0.0f;
    private float m_steerL = 0.0f;
    private float m_steerR = 0.0f;
    private float m_steerAngleMax = 0.0f;
    private float m_maxRollAngle = 45.0f;
    private float throttle;
    //JOHN
    public enum PlatformControllerType
    {
        Accel,
        Mouse,
        Keyboard,
        Analogue,

    };

    public PlatformControllerType SelectedControllerType;



    // JOHN
    public float forwardValue;
    public float reverseValue;
    public float selectedSteerInputAmount;


    public string getGear( )
    {
        return gearInput > 0 ? "D" : gearInput < 0 ? "R" : "N";
    }

    public float getMotor( )
    {
        return motorInput;
    }

    public float getBrake( )
    {
        return brakeInput;
    }

    public float getHandBrake( )
    {
        return handbrakeInput;
    }

    public float getSteerL( )
    {
        return m_steerL;
    }

    public float getSteerR( )
    {
        return m_steerR;
    }

    public float getMaxRollAngle( )
    {
        return m_maxRollAngle;
    }

    void OnEnable( )
    {
        ApplyEnabled(WheelFL, true);
        ApplyEnabled(WheelFR, true);
        ApplyEnabled(WheelRL, true);
        ApplyEnabled(WheelRR, true);
    }

    void OnDisable( )
    {
        ApplyEnabled(WheelFL, false);
        ApplyEnabled(WheelFR, false);
        ApplyEnabled(WheelRL, false);
        ApplyEnabled(WheelRR, false);
    }

    //VCAnalogJoystickBase vcSteer, vcDrive;
    //VCButtonBase accelButton, reverseButton;
    void Start( )
    {


        #if(UNITY_ANDROID)
        SelectedControllerType = PlatformControllerType.Accel;
        #endif
		#if(UNITY_IOS)
		SelectedControllerType = PlatformControllerType.Accel;
		#endif
        //#if(UNITY_EDITOR)
        //        SelectedControllerType = PlatformControllerType.Keyboard;
        //#endif
        //#if(UNITY_WEBPLAYER)
        //        SelectedControllerType = PlatformControllerType.Keyboard;
        //#endif

        // Centro de Masas (CoM)
        // LocalScale es necesario para aplicar cualquier escalado que se haya aplicado al modelo.

        if (CenterOfMass)
            rigidbody.centerOfMass = new Vector3(CenterOfMass.localPosition.x * transform.localScale.x, CenterOfMass.localPosition.y * transform.localScale.y, CenterOfMass.localPosition.z * transform.localScale.z);

        // Punto de equilibrio lateral (estimado)
        WheelCollider WheelC = WheelFL.GetComponent<WheelCollider>() as WheelCollider;
        Vector3 V = rigidbody.centerOfMass - transform.InverseTransformPoint(WheelC.transform.position);
        float h = Mathf.Abs((V.y + WheelC.radius + WheelC.suspensionDistance / 2.0f) * transform.localScale.y);
        float l = Mathf.Abs(V.x * transform.localScale.x);
        m_maxRollAngle = Mathf.Atan2(l, h) * Mathf.Rad2Deg;

        // Otros datos

        rigidbody.maxAngularVelocity = 10;
        rigidbody.useConeFriction = false;

        // En modo optimizado aplicar los parámetros de las ruedas ahora, entonces recalcular los datos costosos

        if (!optimized) return;
        ApplyCommonParameters(WheelFL);
        ApplyCommonParameters(WheelFR);
        ApplyCommonParameters(WheelRL);
        ApplyCommonParameters(WheelRR);
        WheelFL.RecalculateStuff();
        WheelFR.RecalculateStuff();
        WheelRL.RecalculateStuff();
        WheelRR.RecalculateStuff();
    }

    void PressedGas( )
    {
        forwardValue = 1;
    }

    void ReleasedGas( )
    {
        forwardValue = 0;
    }

    void PressedBrake( )
    {
        reverseValue = 1;
    }

    void ReleasedBrake( )
    {
        reverseValue = 0;
    }

    void SteerLeft( )
    {
        steerInput = -1;
        selectedSteerInputAmount = -1;
    }

    void ReleasedSteer( )
    {
        steerInput = 0;
        selectedSteerInputAmount = 0;
    }

    void SteerRight( )
    {
        steerInput = 1;
        selectedSteerInputAmount = 1;
    }

    void FixedUpdate( )
    {
        switch (SelectedControllerType)
        {
            case CarControl.PlatformControllerType.Accel:
                {
                    steerInput = P_HUD.I.steeringWheel._CurrentFloat;
                    selectedSteerInputAmount = steerInput;
                }
                break;
            case CarControl.PlatformControllerType.Keyboard:
                {
                    steerInput = Input.GetAxis("Horizontal");
                    forwardValue = Mathf.Clamp(Input.GetAxis("Vertical"), 0, 1f);
                    reverseValue = -1 * Mathf.Clamp(Input.GetAxis("Vertical"), -1f, 0);
                    selectedSteerInputAmount = steerInput;
                }
                break;
            case CarControl.PlatformControllerType.Mouse:
                {
                    steerInput = ((Input.mousePosition.x / Screen.width) * 2) - 1;
                    forwardValue = Mathf.Clamp(Input.GetAxis("Vertical"), 0, 1);
                    reverseValue = -1 * Mathf.Clamp(Input.GetAxis("Vertical"), -1, 0);
                    selectedSteerInputAmount = steerInput;
                }
                break;
        }
        //======================================================================
        // 1. Aplicar parámetros a las ruedas
        //======================================================================
        // Parámetros de las ruedas que se establecen una vez en el script de control para no tener que ir rueda por rueda

        ApplyCommonParameters(WheelFL);
        ApplyCommonParameters(WheelFR);
        ApplyCommonParameters(WheelRL);
        ApplyCommonParameters(WheelRR);

        // Pámetros para ruedas individuales según configuración actual

        ApplyFrontParameters(WheelFL);
        ApplyFrontParameters(WheelFR);
        ApplyRearParameters(WheelRL);
        ApplyRearParameters(WheelRR);

        //======================================================================
        // 2. Aplicar tracción / frenado
        //======================================================================

        m_motorSlip = motorInput * motorMax;
        m_brakeSlip = brakeInput * brakeMax;

        if (gearInput == 0)
            m_motorSlip = 0;
        else if (gearInput < 0)
            m_motorSlip = -m_motorSlip;

        // Balance de tracción

        if (serviceMode)
        {
            m_frontMotorSlip = m_motorSlip;
            m_rearMotorSlip = m_motorSlip;
        }
        else if (motorBalance >= 0.5f)
        {		// reducir tracción adelante
            m_frontMotorSlip = m_motorSlip * (1.0f - motorBalance) * 2.0f;
            m_rearMotorSlip = m_motorSlip;
        }
        else
        {	// Reducir tracción atrás
            m_frontMotorSlip = m_motorSlip;
            m_rearMotorSlip = m_motorSlip * motorBalance * 2.0f;
        }

        // Balance de frenado

        if (serviceMode)
        {
            m_frontBrakeSlip = m_brakeSlip;
            m_rearBrakeSlip = m_brakeSlip;
        }
        else if (brakeBalance >= 0.5f)
        {  // reducir frenos adelante
            m_frontBrakeSlip = m_brakeSlip * (1.0f - brakeBalance) * 2.0f;
            m_rearBrakeSlip = m_brakeSlip;
        }
        else
        {	// Reducir frenos atrás
            m_frontBrakeSlip = m_brakeSlip;
            m_rearBrakeSlip = m_brakeSlip * brakeBalance * 2.0f;
        }

        ApplyTraction(WheelFL, m_frontMotorSlip, m_frontBrakeSlip, 0.0f);
        ApplyTraction(WheelFR, m_frontMotorSlip, m_frontBrakeSlip, 0.0f);
        ApplyTraction(WheelRL, m_rearMotorSlip, m_rearBrakeSlip, handbrakeInput);
        ApplyTraction(WheelRR, m_rearMotorSlip, m_rearBrakeSlip, handbrakeInput);

        //======================================================================
        // 3. Aplicar dirección
        //======================================================================

        // autoSteerLevel determina el ángulo en el que las ruedas ofrecen la máxima fuerza de giro (peakSlip) a la velocidad actual.
        // Se coge como referencia una rueda cualquiera delantera.

        if (autoSteerLevel > 0.0f)
        {
            float peakSlip = WheelFL.getSidewaysPeakSlip();
            float forwardSpeed = Mathf.Abs(transform.InverseTransformDirection(rigidbody.velocity).z * autoSteerLevel);

            if (forwardSpeed > peakSlip)
                m_steerAngleMax = 90.0f - Mathf.Acos(peakSlip / forwardSpeed) * Mathf.Rad2Deg;
            else
                m_steerAngleMax = steerMax;
        }
        else
            m_steerAngleMax = steerMax;

        // La dirección de giro de cada rueda se calcula en Update para que se visualice con suavidad,
        // incluso en cámara lenta.

        WheelFL.getWheelCollider().steerAngle = m_steerL;
        WheelFR.getWheelCollider().steerAngle = m_steerR;

        //======================================================================
        // 4. Fuerzas de resistencia
        //======================================================================

        // Resistencia aerodinámica (drag) y resistencia a rodar (rr)
        //
        // Fdrag = -Cdrag * V * |V|
        // Frr = -Crr * V

        // Cdrag =  0.5 * Cd * A * rho
        // 	Cd = coeficiente aerodinámico (ej. corvette 0.30)
        //	A = área frontal del vehículo
        //	rho = densidad del aire = 1.29 kg/m3
        //
        // Crr = 30 veces Cdrag (así Fdrag será más importante a partir de 30 m/s = 100 Km/h)

        if (!serviceMode)
        {
            float Cdrag = 0.5f * airDrag * 1.29f; // * (motorMax+1) / 2;
            float Crr = frictionDrag * Cdrag;

            Vector3 Fdrag = -Cdrag * rigidbody.velocity * rigidbody.velocity.magnitude;
            Vector3 Frr = -Crr * rigidbody.velocity;

            rigidbody.AddForce(Fdrag + Frr);
        }

        //======================================================================
        // 5. Ajustes técnicos adicionales
        //======================================================================

        // Barras estabilizadoras

        if (AntiRollFront)
            AntiRollFront.AntiRollFactor = antiRollLevel;
        if (AntiRollRear)
            AntiRollRear.AntiRollFactor = antiRollLevel;
    }

    void Update( )
    {
        // Read user input if specified

        if (readUserInput)
            m_brakeRelease = CarMain.SendInput(this, reverseRequiresStop, m_brakeRelease);

        // Calcular los ángulos de giro de las ruedas para que los cambios de dirección se visualicen con suavemente,
        // incluso en cámara lenta. FixedUpdate coge los valores que haya en el momento.

        CalculateSteerAngles();
    }

    void ApplyEnabled(CarWheel Wheel, bool enable)
    {
        // Necesario comprobar con null, ya que el OnDisable se invoca al finalizar la aplicación,
        // y el objeto puede no estar ya disponible aunque su referencia sea no nula.

        if (Wheel != null)
            Wheel.enabled = enable;
    }

    void ApplyCommonParameters(CarWheel Wheel)
    {
        Wheel.ForwardWheelFriction = ForwardWheelFriction;
        Wheel.SidewaysWheelFriction = SidewaysWheelFriction;
        Wheel.brakeForceFactor = brakeForceFactor;
        Wheel.motorForceFactor = motorForceFactor;
        Wheel.performancePeak = motorPerformancePeak;
        Wheel.sidewaysDriftFriction = sidewaysDriftFriction;
        Wheel.staticFrictionMax = staticFrictionMax;

        Wheel.serviceMode = serviceMode;
        Wheel.optimized = optimized;
    }

    void ApplyFrontParameters(CarWheel Wheel)
    {
        Wheel.sidewaysForceFactor = frontSidewaysGrip;
    }

    void ApplyRearParameters(CarWheel Wheel)
    {
        Wheel.sidewaysForceFactor = rearSidewaysGrip;
    }

    void ApplyTraction(CarWheel Wheel, float motorSlip, float brakeSlip, float handBrakeInput)
    {
        float slipPeak = Wheel.getForwardPeakSlip();
        float slipMax = Wheel.getForwardMaxSlip();

        WheelHit Hit = new WheelHit();
        float slip;

        // Tracción

        float motor = Mathf.Abs(motorSlip);	// motor = [0..motorMax]

        bool grounded = Wheel.getWheelCollider().GetGroundHit(out Hit);

        if (grounded)
        {
            Quaternion steerRot = Quaternion.AngleAxis(Wheel.getWheelCollider().steerAngle, Wheel.transform.up);
            Vector3 wheelDir = steerRot * Wheel.transform.forward;

            Vector3 pointV = rigidbody.GetPointVelocity(Hit.point);

            if (Hit.collider.attachedRigidbody)
                pointV -= Hit.collider.attachedRigidbody.GetPointVelocity(Hit.point);

            float v = Mathf.Abs(Vector3.Dot(pointV, wheelDir));

            if (v + slipPeak <= motorMax)
            {
                slip = motor - v;

                if (slip < 0)
                    slip = 0;
                else if (autoMotorMax && slip > slipPeak)
                    slip = slipPeak;
            }
            else
            {
                float maxSlip;

                if (tractionV3)
                    maxSlip = Mathf.Lerp(slipPeak, 0, Mathf.InverseLerp(motorMax - slipPeak, maxSpeed, v));
                else
                    maxSlip = slipPeak;
                slip = maxSlip * motor / motorMax;
            }

            if (motorSlip < 0)
                slip = -slip;
        }
        else
            slip = motorSlip;

        // Frenos

        if (autoBrakeMax && brakeSlip > slipPeak)
            brakeSlip = slipPeak;

        brakeSlip = Mathf.Max(brakeSlip, handBrakeInput * slipMax);

        if (motorInput == 0.0f)
            brakeSlip += rollingFrictionSlip / brakeForceFactor;

        if (!grounded)
        {  // Las ruedas en el aire se frenan en función de su velocidad
            float omega = Wheel.getWheelCollider().rpm * Mathf.Deg2Rad;
            brakeSlip += omega * omega * 0.0008f / brakeForceFactor;
        }

        Wheel.motorInput = slip;
        Wheel.brakeInput = brakeSlip;
    }

    void CalculateSteerAngles( )
    {
        // -------- Giro de dirección ruedas delanteras

        // Calcular el ángulo de ambas ruedas para el giro perfecto.
        // Se asume que las distancias entre las ruedas son iguales delante-detrás e izquierda-derecha.
        // Valores máximos de giro: -90..+90

        float B = (WheelFL.transform.position - WheelFR.transform.position).magnitude;
        float H = (WheelFR.transform.position - WheelRR.transform.position).magnitude;

        if (steerInput > 0.0f)
        {			// Giro a la derecha
            m_steerR = steerMax * steerInput;
            if (m_steerR > m_steerAngleMax)
                m_steerR = m_steerAngleMax;
            m_steerL = Mathf.Rad2Deg * Mathf.Atan(1.0f / (Mathf.Tan((90 - m_steerR) * Mathf.Deg2Rad) + B / H));
        }
        else if (steerInput < 0.0f)
        {		// Giro a la izquierda
            m_steerL = steerMax * steerInput;
            if (m_steerL < -m_steerAngleMax)
                m_steerL = -m_steerAngleMax;

            m_steerR = -Mathf.Rad2Deg * Mathf.Atan(1.0f / (Mathf.Tan((90 + m_steerL) * Mathf.Deg2Rad) + B / H));
        }
        else
        {
            m_steerL = 0;
            m_steerR = 0;
        }
    }

    void Drive(float throttleChangeSpeed, float throttle)
    {
        motorInput = Mathf.MoveTowards(motorInput, throttle, throttleChangeSpeed * Time.deltaTime);
        gearInput = 1;
        brakeInput = 0;
        handbrakeInput = 0.0f;
    }
}