using UnityEngine;
using System.Collections;
using System.Collections.Generic;

/*
public class CarWheelHit{
	public Vector3 point;
	public Vector3 normal;
}

public class CarWheelCollider : MonoBehaviour {
	public float wheelRadius = 0.33f;
	public float _steerAngle;

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


	private Rigidbody _chassisRigidBody;
*/
	/*private CarWheelHit _hit = new CarWheelHit();
	private bool _isGrounded;
	private Vector3 _relativePosition; //Relative to car body
	private Vector3 _velocity;
	private Vector3 _localVelocity;
	public float camber;
	private Vector3 _friction = new Vector3 ();
	public float _appliedTorque;
	private Vector2 _slip; //X = lateral, Y = longitudinal
	private Vector3 _peakSlip;
	private float _driveTorque;
	private float _breakTorque;*/
	
/*
	private DriveShaft _driveShaft = new DriveShaft();


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

	/*public float SteerAngle {
		get {
			return _steerAngle;
		}
		set {
			_steerAngle = value;
		}
	}*/

/*void SetTorque(float torque, float dt){
    _driveShaft.applyMomentum(torque * dt);
}

float RPM{
    get{return _driveShaft.AngularVelocity * 30.0f / 3.141593f;}
}

void Integrate(float dt){
    _driveShaft.integrate(dt);
}
	
private void UpdateRelativePosition(){
    _relativePosition = _chassisRigidBody.transform.InverseTransformPoint (transform.position);
}

// Use this for initialization
void Start () {
    _chassisRigidBody = FindFirstParentRigidBody ();
    UpdateRelativePosition ();


    //_breakTorque = 10000;
    float tire_mass = 4.0f;
    float rim_mass = 15.0f;
    float rim_radius = 0.331f;
    float tire_radius = 0.556f;

    float tire_inertia = tire_mass * tire_radius * tire_radius;
    float rim_inertia = rim_mass * rim_radius * rim_radius;
    SetInertia(tire_inertia + rim_inertia);
}

void SetInertia(float value)
{
    _driveShaft.Inv_inertia = 1.0f / value;
}

private void UpdateSlipAngles(){
    _localVelocity = transform.InverseTransformDirection (_velocity);
    Quaternion q = Quaternion.AngleAxis (-SteerAngle, transform.up);
    var trs = Matrix4x4.TRS (Vector3.zero, q, Vector3.one);
    _localVelocity = trs.MultiplyPoint3x4 (_localVelocity);

    float rot_velocity = AngularVelocity * wheelRadius;
    float lon_velocity = _localVelocity.z;
    float lat_velocity = _localVelocity.x;
    float lon_slip_velocity = lon_velocity - rot_velocity;

    if (Mathf.Abs (lon_slip_velocity) <= 1.0e-4f)
    {
        _slip = Vector2.zero;
        return;
    }

    float denom = Mathf.Max(Mathf.Abs(lon_velocity), 1e-0f);
    _slip.x = -lon_slip_velocity / denom;
    _slip.y = Mathf.Atan(lat_velocity / denom);
}

private void UpdateFriction(){
    float surfaceFriction = 0.9f;

    var normalForce = 4000.0f;// Mathf.Clamp(-_suspensionForce, 0, 10000);
    if(normalForce * surfaceFriction < 0.00001f){
        _peakSlip = Vector3.one;
        _friction = Vector3.zero;
        return;
    }

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

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

    float epsilon = 0.000001f;

    Vector3 slip = new Vector3();
    if(Mathf.Abs(_peakSlip.x) >= epsilon){
        slip.x = _slip.x / _peakSlip.x; 
    }
    if(Mathf.Abs(_peakSlip.y) >= epsilon){
        slip.y = _slip.y / _peakSlip.y; 
    }
    if(Mathf.Abs(_peakSlip.z) >= epsilon){
        slip.z = _slip.y / _peakSlip.z; 
    }


    //Combined friction (friction circle)
    float slip_xy = new Vector3 (slip.x, slip.y, 0.0f).magnitude;
    float slip_xz = new Vector3 (slip.x, 0.0f, slip.z).magnitude;


    if ((Mathf.Abs(normalForce) > epsilon) && (_localVelocity.magnitude > 0.001f)) {
        _friction.x = Pacejka.MagicFormula(Mathf.Sign(slip.x) * slip_xy * _peakSlip.x, longitudinalParams) * surfaceFriction;
        _friction.y = Pacejka.MagicFormula(Mathf.Sign(slip.y) * slip_xy * _peakSlip.y, lateralParams) * surfaceFriction;
        _friction.z = Pacejka.MagicFormula(slip_xz * _peakSlip.z, aligningParams) * surfaceFriction;

        if (slip_xy > epsilon)
        {
            _friction.x *= Mathf.Abs (slip.x) / slip_xy;
            _friction.y *= Mathf.Abs (slip.y) / slip_xy;
        }
        if (slip_xz > epsilon)
            _friction.z *= slip.z / slip_xz;
    }else{
        _friction = Vector3.zero;
    }
}

public float AngularVelocity{
    get{return _driveShaft.AngularVelocity;}
}

public void FixedUpdate(){
    UpdateRelativePosition ();


    Quaternion q = Quaternion.AngleAxis (SteerAngle, transform.up);
    var trs = Matrix4x4.TRS (Vector3.zero, q, Vector3.one);
    var right = trs.MultiplyPoint3x4 (transform.right).normalized;
    var forward = trs.MultiplyPoint3x4 (transform.forward).normalized;

    //get wheel joint velocity
    _velocity = _chassisRigidBody.GetPointVelocity (transform.position);


    UpdateSlipAngles ();
    UpdateFriction ();


*/


/*var raycastRay = new Ray (transform.position, -transform.up);

RaycastHit rayHit;
if (Physics.Raycast (raycastRay, out rayHit)) {
    if(rayHit.distance <= (wheelRadius + suspensionDistance)){
        _isGrounded = true;
        currentLength = 1.0f - (rayHit.distance - wheelRadius) / suspensionDistance;


        _suspensionForce = suspensionSpring * (targetPosition - currentLength) + suspensionDamper * (previousLength - currentLength) / Time.fixedDeltaTime;
        //_chassisRigidBody.AddForceAtPosition(rayHit.normal * -_suspensionForce, transform.position);
				
                
        previousLength = currentLength;
        _hit.point = rayHit.point;
        _hit.normal = rayHit.normal;




        var friction = forward * _friction.x- right * _friction.y;
        var frictionTorque = _friction.x * wheelRadius;

        var reaction_torque = frictionTorque;
        var applied_torque = _driveTorque + _breakTorque;

        if ( ( applied_torque > 0 && reaction_torque > applied_torque ) ||
            ( applied_torque < 0 && reaction_torque < applied_torque ) )
            reaction_torque = applied_torque;

        var tire_torque = right * reaction_torque;

        float matFriction = 0.8f;
        float rolling = 0;// matFriction * (0.0136f + 0.4e-7f * AngularVelocity * AngularVelocity);

        float rolling_resistance_torque = rolling * wheelRadius - frictionTorque;
        float wheel_torque = _driveTorque + _breakTorque + rolling_resistance_torque;


        this.SetTorque(wheel_torque, Time.fixedDeltaTime);
        this.Integrate(Time.fixedDeltaTime);

		

        _chassisRigidBody.AddForceAtPosition(friction, rayHit.point);
        //_chassisRigidBody.AddForceAtPosition(forward * _friction.x, transform.position);

    }else{
        currentLength = suspensionDistance;
        previousLength = currentLength;
        _isGrounded = false;
        _friction = Vector3.zero;

    }
		
}
else{
    currentLength = suspensionDistance;
    previousLength = currentLength;
    _isGrounded = false;
    _friction = Vector3.zero;
}

_currentSuspensionPosition = currentLength;*/
	/*}

	private Rigidbody FindFirstParentRigidBody(){
		var currentTransform = transform;
		while (true) {
			var parent = currentTransform.parent;
			if(parent == null){
				return null;
			}

			var rb = parent.gameObject.GetComponent<Rigidbody>();
			if(rb != null){
				return rb;
			}
			currentTransform = parent;
		}
	}
	
	// Update is called once per frame
	void Update () {
	
	}

	void OnGUI(){
		int id = int.Parse (name [name.Length - 1].ToString ());
		SteerAngle = GUI.HorizontalSlider (new Rect(10, 20 * id, 100,20), SteerAngle, -30, 30);
		GUI.Label (new Rect (120, 20 * id, 100, 20), SteerAngle.ToString());
	}
	

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

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

		int sampleCnt = 150;
		float minArg = -20;
		float maxArg = 20;
		float argDiff = maxArg - minArg;

		for (int i = 0; i < sampleCnt; ++i) {
			var arg = minArg + argDiff / ((float)sampleCnt - 1) * (float)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));
		}


		Tools.BeginGui ();
		Plot2D plot = new Plot2D(curves[1]);

		//Slip.peak_slip(

		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 ();
	}*/

	//void OnDrawGizmos(){

		//_suspensionForce = 5000.0f;
		//UpdateSlipAngles ();
		//UpdateFriction ();


		/*if (suspensionDistance < 0.01f) {
			suspensionDistance = 0.01f;
		}*/
		/*if (wheelRadius < 0.01f) {
			wheelRadius = 0.01f;
		}
		//targetPosition = Mathf.Clamp01 (targetPosition);

		Vector3 wheelPosition;
		Vector3 bottom;
    */
		/*if (Application.isPlaying) {
			wheelPosition = transform.position - transform.up.normalized * (1 - _currentSuspensionPosition) * suspensionDistance;

		}else{
			wheelPosition = transform.position;
		}*/


		//bottom = transform.position - transform.up.normalized * suspensionDistance;

		/*float kSuspW = 0.1f;
		float suspHW = wheelRadius * kSuspW * 0.5f;

		Quaternion q = Quaternion.AngleAxis (SteerAngle, transform.up);
		var trs = Matrix4x4.TRS (Vector3.zero, q, Vector3.one);
		var right = trs.MultiplyPoint3x4 (transform.right);
		var forward = trs.MultiplyPoint3x4 (transform.forward);
    */

		/*Gizmos.color = Color.red;
		//draw suspension 
		Gizmos.DrawLine (transform.position, bottom);
		Gizmos.DrawLine (transform.position - forward * suspHW * 1.5f, transform.position + forward * suspHW* 1.5f);
		Gizmos.DrawLine (bottom - forward * suspHW, bottom + forward * suspHW);
        */


		/*var colliderColor = new Color (0, 1, 0, 0.5f);
		Gizmos.color = colliderColor;


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

    */


		//draw wheel circle
		//AdvancedGizmo.DrawCircle (wheelCenter, right, wheelRadius, colliderColor, 36);

		//draw force point
		/*var wheelBottom = wheelCenter - transform.up * wheelRadius;
		AdvancedGizmo.DrawCircle (wheelBottom, forward, wheelRadius * kSuspW, colliderColor);
		AdvancedGizmo.DrawCircle (wheelBottom, right, wheelRadius * kSuspW, colliderColor);
		AdvancedGizmo.DrawCircle (wheelBottom, transform.up, wheelRadius * kSuspW, colliderColor);
    */
		//draw friction
		/*Gizmos.color = Color.red;
		Gizmos.DrawLine (transform.position, transform.position + forward * _friction.x);

		//draw wheel velocity
		Gizmos.color = colliderColor;
		Gizmos.DrawLine (transform.position, transform.position + _velocity);

		var p = Camera.current.WorldToScreenPoint (transform.position + Vector3.up);
		p.y = Camera.current.pixelHeight - p.y;



	
		Tools.BeginGui ();
		var labelRect = new Rect (p.x, p.y, 90, 20);
		Tools.DrawRectangle (labelRect, new Color(0,0,0,0.5f));
		GUI.Label (new Rect (p.x, p.y, 100, 100), _friction.x.ToString ());

		Tools.EndGui ();
	}*/
//}