using UnityEngine;
using System.Collections;

public abstract class Suspension : MonoBehaviour {
	
	private double _steer;
	protected double _normalForce;
	public WheelPhysics wheel;
	public double travelDistance = 0.2f;
	public double suspensionTarget = 0.2f;
	private double _displacement;
	protected double _previousFraction;
	private double _overtravel;
	private double _lastDisplacement;
	public double springForce = 9000;
	public double springDamping = 400;

	public double Steer{
		get{return _steer;}
	    set
	    {
	        _steer = value;
	        OnSteerChanged();
	    }
	}

    public virtual void OnSteerChanged() {

    }
	
	// Use this for initialization
	void Start () {
		
	}

	public double Displacement {
		get{return _displacement;}
		set{
			_lastDisplacement = _displacement;
			_displacement = MathEx.Clamp(value, 0, travelDistance);
			_overtravel = value - travelDistance;
			if(_overtravel < 0){
				_overtravel = 0;
			}
		}
	}

	public void CalculateForce(double dt){
		double velocity = ((_displacement - _lastDisplacement) / travelDistance) / dt;
		double spring = (_displacement / travelDistance) * springForce;
		double damping = velocity * springDamping;
		_normalForce = damping + spring;
	}


	double computeImpulseDenominator(Vector3 pos, Vector3 normal, Rigidbody rb){
		Vector3 r0 = pos - rb.worldCenterOfMass;
		Vector3 c0 =  Vector3.Cross(r0, normal);


		var tensorLocal = Matrix4x4.TRS(rb.worldCenterOfMass, rb.inertiaTensorRotation, rb.inertiaTensor);
		var tensorWorld = transform.localToWorldMatrix * tensorLocal;
		tensorWorld = Matrix4x4.Inverse(tensorWorld);
		Vector3 vec = Vector3.Cross(tensorWorld.MultiplyPoint(c0), r0);
		
		return 1.0f / rb.mass + Vector3.Dot(normal, vec);
	}

    protected Vector3 dbgWheelForce;
    protected Vector3 dbgWheelForcePos;

    public void Integrate(double dt, Car car){
		if(wheel != null){
			//var rb = car.GetRigidBody();

			//raycast wheel
			wheel.UpdateContactPoint(car.transform.up);
			double fraction;
            double relativeDisplacement = wheel.TotalWheelRadius - wheel.Contact.hit.distance;
			Displacement += relativeDisplacement;

			fraction = _displacement / travelDistance;
			CalculateForce(dt);

			var wheelWorldVelocity = car.GetRigidBody().GetPointVelocity(wheel.transform.position);

			if (_overtravel > 0)
			{
                double correction_factor = 0.0f;
                double dv = Vector3.Dot(wheelWorldVelocity, transform.up);// body->getVelocityInLocalPoint(suspension_force_application_point).dot(forcedirection);
				dv -= correction_factor * _overtravel / dt;
              //  double effectiveMass = 1.0 / computeImpulseDenominator(wheel.transform.position, transform.up, car.GetRigidBody());//1.0 / body->computeImpulseDenominator(wheel_position[i], forcedirection);
              //  double correction = -effectiveMass * dv / dt;
				/*if (correction > 0)
				{
					_normalForce = correction;
				}*/
			}

            if(!wheel.Contact.hasHit) {
                _normalForce = 0;
            }

            if(_normalForce > 0.0f){
				car.GetRigidBody().AddForceAtPosition(wheel.Contact.hit.normal * (float)_normalForce, transform.position);
			}

			wheel.transform.position = GetWheelPosition(fraction);

			Vector3 wheelForce;
			wheel.Integrate(dt, wheelWorldVelocity, _normalForce, out wheelForce);
			car.GetRigidBody().AddForceAtPosition(wheelForce, wheel.transform.position);

		    dbgWheelForce = wheelForce;
		    dbgWheelForcePos = wheel.transform.position;

		}
	}


    public void CalculateSuspensionForce(double dt, double fraction) {
        _normalForce = springForce * (suspensionTarget - fraction) + springDamping * (_previousFraction - fraction) / dt;
        _previousFraction = fraction;
    }

    public Vector3 GetWheelPosition(double fraction) {
        var up = transform.up;
        var center = transform.position;
        var top = center + up * (float)((1.0 - suspensionTarget) * travelDistance);
        var bottom = center - up * (float)(suspensionTarget * travelDistance);

        return bottom + (top - bottom) * Mathf.Clamp01((float)fraction);

    }



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