using UnityEngine;
using UnityEditor;
using System.Collections;
using System;

[ExecuteInEditMode]
public class Tyre : MonoBehaviour {
    public double height = 0.15f;
    public double k_RollLinear = 0.0038;
    public double k_RollQuad = 0.000026;

    public double GetRollingResistance(double Fz, double wheelEffectiveRadius, double velocity, double resistanceFactor) {
        return Fz * wheelEffectiveRadius * resistanceFactor * (k_RollLinear + k_RollQuad * (velocity )) * Math.Sign(velocity);
    }

    private double oldLatSlip;
    public double latFilter = 0.85f;


    public PacejkaParameters pacejkaParams;

 

   /* public void Slip(double patch_speed, Vector3 hub_velocity, out double slipLon, out double slipLat) {
        var absLongVelocity = (double)Mathf.Abs(hub_velocity.z);
        absLongVelocity = Math.Max(absLongVelocity, dampAbsRoadVelo);
        slipLon = CalcSlipRatio(patch_speed, hub_velocity) * 100.0f;
        slipLat = (Mathf.Atan2(hub_velocity.x, (float)absLongVelocity)) * Mathf.Rad2Deg;
    }

    public Double3 FrictionForce(double normalForce, double slipLon, double slipLat, double camber, PacejkaCoeffs coeffs) {
        if(normalForce < 1e-4f) { return Double3.zero; }

        normalForce = MathEx.Clamp(normalForce, 0, 100000);

        //compute Pacejka params
        var lateralParams = Pacejka.LateralParams(normalForce, camber, coeffs);
        var longitudinalParams = Pacejka.LongitudinalParams(normalForce, coeffs);
        var aligningParams = Pacejka.AligningParams(normalForce, camber, coeffs);

        //compute friction forces
        Double3 result;
        result.x = Pacejka.MagicFormula(slipLon, longitudinalParams);
        result.y = Pacejka.MagicFormula(slipLat, lateralParams);
        result.z = Pacejka.MagicFormula(slipLat, aligningParams);

       /* //do combined friction
        double d_lon = slipLon / (1.0 + slipLon);
        double d_lat = Math.Tan(slipLat) / (1.0 + slipLon);
        double d = Math.Sqrt(d_lat * d_lat + d_lon * d_lon);

        if(d > 0.001) {
            result.x *= (float)Math.Abs(d_lon / d);
            result.y *= (float)Math.Abs(d_lat / d);
        }*/
       // return result;
   // }


    void OnValidate() {
        if(enabled) {
            if(transform.parent != null && gameObject.activeInHierarchy)
            {
                SceneView.RepaintAll();
                //   transform.parent.gameObject.SendMessage("OnValidate");
            }
        }
    }


    // SAE950311 implementation
   // private double lastU;
  //  private double signU = 1;
     //private double oldSlip = 0;
 //   public double tau;
   // public double B = 0.091f;//relaxationLengthLong
    public double relaxLong = 0.00172688486f;
  //  private double _integratedSlip;
    public double speedThreshold = 3.0f;
    public double dampAbsRoadVelo = 1;
    public double kBLong = 0.35f;

    public double fixedTimeBase = 0.02f;


    public double CalcSlipRatio(double patch_speed, Vector3 hub_velocity) {
        bool IsOnSurface = true;
        if(!IsOnSurface) {
           // _integratedSlip = 0;
            return 0;
        }

      /*  double deltaTime = Time.fixedDeltaTime;
        double dampSRreversal = 0.5f;
        double B = relaxLong*100;
        double fixedTimeStepScalar = fixedTimeBase / Time.fixedDeltaTime;
        double invFixedTimeStepScalar = 1.0f / fixedTimeStepScalar;

        if(B < kBLong * invFixedTimeStepScalar)
            B = kBLong * invFixedTimeStepScalar;

        // damp sliAngle and slipRatio oscilation


        double factor = hub_velocity.z * 3.6f * 0.02f; // divided by 50
        if(factor < 1)
            factor = 1;
        B *= factor;
        */

        // SAE950311 algorithm
      
        var longVelocity = (double)hub_velocity.z;
      //  var absLongVelocity = Math.Abs(longVelocity);



        if (Math.Abs(longVelocity - patch_speed) > 1.0e-4)
        {
            double denom = Math.Max(Math.Abs(longVelocity), dampAbsRoadVelo);
            return MathEx.Clamp((patch_speed - longVelocity)/denom, -1.0, 1.0);
        }
        else
        {
            return 0;
        }



/*
        absLongVelocity = Math.Max(absLongVelocity, dampAbsRoadVelo);

       


        if((lastU < 0 && longVelocity >= 0) || (lastU >= 0 && longVelocity < 0)) {
            _integratedSlip = -dampSRreversal * _integratedSlip;
        }

        lastU = Math.Sign(longVelocity);

        double derivative = (patch_speed - longVelocity) - absLongVelocity * _integratedSlip;
        derivative /= B;

        // Integrate
        _integratedSlip += derivative * deltaTime;

        return MathEx.Clamp(_integratedSlip + tau * derivative, -1.5, 1.5);*/
    }
}