/*
using UnityEngine;


[ExecuteInEditMode]
public class Wheel : MonoBehaviour {
   
    public Tyre tyre;
    public Brake brake;

    public DriveShaft _shaft;

     public float discRadius = 0.385f; //15 inch by default
    private float _totalWheelRadius;

	public float TotalWheelRadius {
		get {
			return _totalWheelRadius;
		}
	}

    public float _slipLon;
    public float _slipLat;
    private Vector3 _localVelocity;
    private Vector3 _friction;
    private Vector3 _peakSlip;
    public float camber;
    public bool DrawDebug = true;
    public float weight = 50;

    public float driveTorque = 0.0f;
    public float brakeTorque = 0.0f;
   

    public float brakingState = 0.0f;
	private WheelContact _contact;
    public Oscilloscope oscilloscope;
    private int t;

	public bool Grounded {
		get {
			return _contact.grounded;
		}
	}

	public WheelContact Contact {
		get {
			return _contact;
		}
	}



    [ContextMenuItem("Reset coeffs", "resetPacejkaCoeffs")]
    public PacejkaCoeffs _coeffs = new PacejkaCoeffs();

    public float dtMul = 1;

    private void resetPacejkaCoeffs() {
        _coeffs = new PacejkaCoeffs();
    }

    public float AngularVelocity {
        get { return _shaft.AngularVelocity; }
        set {
            _shaft.AngularVelocity = value;
        }
    }

	// Use this for initialization
	void Start () {
	    InitWheel();
        float normalForce = 3.210758f * 1000.0f;
        var lateralParams = Pacejka.LateralParams(normalForce, 0, _coeffs);
        var longitudinalParams = Pacejka.LongitudinalParams(normalForce, _coeffs);
        var aligningParams = Pacejka.AligningParams(normalForce, 0, _coeffs);

        _peakSlip.x = Pacejka.Extremum(longitudinalParams);
        _peakSlip.y = Pacejka.Extremum(lateralParams);
        _peakSlip.z = Pacejka.Extremum(aligningParams);

        if(oscilloscope != null)
        {
            oscilloscope.channels[0].sampler = Sampler0;
            oscilloscope.channels[1].sampler = Sampler1;
            oscilloscope.channels[2].sampler = Sampler2;
        }
    }

    float Sampler0()
    {
        return _slipLat;
    }

    float Sampler1()
    {
        return localVelocity.x;
    }

    float Sampler2()
    {
        return 0;//Mathf.Repeat(_shaft.Angle, Mathf.PI * 2);
    }

    float Inertia{
		get{return 1.0f / _shaft.Inv_inertia;}
		set{_shaft.Inv_inertia = 1.0f / value;}
	}

    void InitWheel() {
        if (_shaft == null) {
            _shaft = new DriveShaft();
        } 
        

        if (tyre != null) {
            _totalWheelRadius = discRadius + tyre.height;
        }

		Inertia = weight*_totalWheelRadius*_totalWheelRadius*0.5f;

    }
    void OnValidate() {
        InitWheel();
    }


    public void SetBraking(float amount) {
        brakingState = Mathf.Clamp01(amount);
      
    }

    void SetTorque(float torque, float dt) {
        _shaft.ApplyMomentum(torque * dt);
    }


    void OnDrawGizmosSelected() {
		if (!DrawDebug) {
			return;
		}
		var p = _coeffs;

		Curve[] curves = new Curve[]{new Curve(), new Curve(), new Curve()};

		int sampleCnt = 250;
		float minArg = -100;
		float maxArg = 100;
		float argDiff = maxArg - minArg;

		for (int i = 0; i < sampleCnt; ++i) {
			var arg = minArg + argDiff / ((float)sampleCnt - 1) * i;
			curves[0].AddArg(arg);
			curves[1].AddArg(arg);
			curves[2].AddArg(arg);

			curves[0].AddVal(Pacejka.LongitudinalForce (5000.0f, arg, p));
			curves[1].AddVal(Pacejka.LateralForce (5000.0f, camber, arg, p));
			curves[2].AddVal(Pacejka.AligningForce (5000.0f, camber, arg, p));
		}

        float maxSlip = MathEx.FindExtremumValue(x => Pacejka.LongitudinalForce(5000.0f, x, p), 0, 100, 0.5f);

        Tools.BeginGui ();

        GUI.Label(new Rect(20,30,300,100), "Maximum wheel slip: " + maxSlip + "  Value at that slip: " + Pacejka.LongitudinalForce(5000.0f, maxSlip, p));
		Plot2D plot = new Plot2D(curves[0]);

		plot.Title = "Longitudinal friction";
		plot.OnGUI(new Rect(90,100,500,400));

		camber = GUI.HorizontalSlider (new Rect (10, 10, 100, 10), camber, -10, 10);
		Tools.EndGui ();
	}

	float GetTorque(float new_angvel, float dt) {
		return _shaft.GetMomentum(new_angvel) / dt;
	}


	private float roll_resistance_lin = 0.00001f;
	private float roll_resistance_quad = 0.00001f;
	float getRollingResistance(
		float velocity,
		float resistance_factor)
	{
		// surface influence on rolling resistance
		float rolling_resistance = resistance_factor * roll_resistance_lin;
		
		// heat due to tire deformation increases rolling resistance
		// approximate by quadratic function
		rolling_resistance += velocity * velocity * roll_resistance_quad;
		
		// rolling resistance direction
		float resistance = -rolling_resistance;
		if (velocity < 0) resistance = -resistance;
		
		return resistance;
	}


	public void UpdateContactPoint(Vector3 up){
		var raycastRay = new Ray (transform.position, -up);

		_contact.hasHit = false;
		_contact.grounded = false;
		if (Physics.Raycast (raycastRay, out _contact.hit)) {
			_contact.hasHit = true;
			if(_contact.hit.distance <= _totalWheelRadius){
				_contact.grounded = true;
			}
		}
	}


	int pp = 0;
    private Vector3 lonForce;
    private Vector3 latForce;
    private float wheel_torque;
    public float dampOsc = 0.85f;
    public float fixedTimeBase = 0.02f;
    private Vector3 localVelocity;

    public void Integrate(float dt, Vector3 wheelWorldVelocity, float normal_force, out Vector3 force)
    {
        //normal_force = 5000.0f;
        float wheel_drive_torque = driveTorque;
		float patch_speed = AngularVelocity * _totalWheelRadius;
        localVelocity = transform.InverseTransformDirection(wheelWorldVelocity);

        tyre.Slip(patch_speed, localVelocity, out _slipLon, out _slipLat);

		Vector3 friction_force = tyre.FrictionForce(normal_force, _slipLon, _slipLat, 0, _coeffs);

        lonForce = transform.forward * friction_force[0];
        latForce =  -transform.right * friction_force[1];

        Vector3 tire_force = lonForce + latForce;

        float tire_friction_torque = friction_force[0] * _totalWheelRadius;
		
		//calculate brake torque
		float wheel_lock_torque = -AngularVelocity / dt * Inertia;
		float wheel_brake_torque = wheel_lock_torque - wheel_drive_torque + tire_friction_torque;
		float maxBreakTorque = brakeTorque;
		if (wheel_brake_torque > 0 && wheel_brake_torque > maxBreakTorque)
		{
			wheel_brake_torque = maxBreakTorque;
		}
		else if (wheel_brake_torque < 0 && wheel_brake_torque < -maxBreakTorque)
		{
			wheel_brake_torque = -maxBreakTorque;
		}
		
		//limit the reaction torque to the applied drive and braking torque
		float reaction_torque = tire_friction_torque;
		float applied_torque = wheel_drive_torque + wheel_brake_torque;
		if ( ( applied_torque > 0 && reaction_torque > applied_torque ) ||
		    ( applied_torque < 0 && reaction_torque < applied_torque ) )
			reaction_torque = applied_torque;
		Vector3 tire_torque = Vector3.right * reaction_torque;// - direction::up * friction_force[2];
		
		//set wheel torque due to tire rolling resistance
		float rollResistanceCoefficient = 0;
        float rolling_resistance = getRollingResistance(AngularVelocity, rollResistanceCoefficient);
        float rolling_resistance_torque = rolling_resistance * _totalWheelRadius - tire_friction_torque;


        wheel_torque = wheel_drive_torque + wheel_brake_torque + rolling_resistance_torque;

        //have the wheels internally apply forces, or just forcibly set the wheel speed if the brakes are locked

        float oldAngularVelocity = AngularVelocity;
        SetTorque(wheel_torque, dt);

        / *float fixedTimeStepScalar = fixedTimeBase / Time.fixedDeltaTime;
        float invFixedTimeStepScalar = 1.0f / fixedTimeStepScalar;

        // damp low speed wheel oscillations
        float deltaAngularVelocity = AngularVelocity - oldAngularVelocity;
        float delta = deltaAngularVelocity * dampOsc * Mathf.Clamp01(invFixedTimeStepScalar);
        AngularVelocity -= delta;* /

        _shaft.Integrate(dt);
		
		//viscous tire contact drag (hack)
		float rollingDrag = 0.0f;
		Vector3 wheel_drag = _localVelocity * rollingDrag;


		force = Vector3.zero;
		//apply forces to body
//		Vector3 tire_pos = wheel_position[i] - body->getCenterOfMassPosition();
		Vector3 world_tire_force = tire_force;
        //Vector3 world_tire_torque = quatRotate ( wheel_orientation[i],  tire_torque);
        world_tire_force += wheel_drag;
        //world_tire_torque += tire_pos.cross(world_tire_force);
        force =  world_tire_force;

      
		//torque = torque + world_tire_torque;
    }

    float RPM
	{
		get {
            return _shaft.AngularVelocity * 30.0f / 3.1415926535f;
        }
	}

    public DriveShaft Shaft {
        get {
            return _shaft;
        }
    }

    void OnGUI()
    {
        DrawLabels();
    }

    void DrawLabels() {
        Label3D.AtWorldPosition(transform.position + Vector3.up * 1, "RPM: " + RPM);
        Label3D.AtWorldPosition(transform.position + Vector3.up * 1.3f, "F: " + _friction.x + " S: " + _slipLon);
    }

    void OnDrawGizmos() {

        if (tyre == null || _shaft == null) {
            return;
        }
        DrawLabels();
        var forward = transform.forward;
        var right = transform.right;

        //draw wheel radius
		var wheelCenter = transform.position;
        var rollMatrix = Matrix4x4.TRS(Vector3.zero, Quaternion.AngleAxis(_shaft.Angle * Mathf.Rad2Deg, right), Vector3.one);
        var radiusP0 = -forward * _totalWheelRadius;
        var radiusP1 = forward * _totalWheelRadius * 1.2f;
		radiusP0 = rollMatrix.MultiplyPoint3x4 (radiusP0);
		radiusP1 = rollMatrix.MultiplyPoint3x4 (radiusP1);
		Gizmos.DrawLine (wheelCenter + radiusP0, wheelCenter + radiusP1);

		//draw wheel outer circle
        AdvancedGizmo.DrawCircle(wheelCenter, right, _totalWheelRadius, Color.green, 36);

        //draw wheel inner circle
        AdvancedGizmo.DrawCircle(wheelCenter, right, discRadius, Color.green, 36);

        //draw forces
        Gizmos.color = Color.red;
        Gizmos.DrawLine(wheelCenter, wheelCenter + lonForce);

        Gizmos.color = Color.green;
        Gizmos.DrawLine(wheelCenter, wheelCenter + latForce);

        Gizmos.color = Color.blue;
        Gizmos.DrawLine(wheelCenter, wheelCenter + latForce + lonForce);

        Gizmos.color = Color.black;
        Gizmos.DrawLine(wheelCenter, wheelCenter + forward * 100);

    }
}
*/