using UnityEngine;
using System.Collections;
using System;

public enum CarPedal{
	Clutch = 0,
	Brake = 1,
	Accelerator = 2
}

[SelectionBase]
public class Car : MonoBehaviour {
	public Chassis chassis;
	public SteeringWheel SteeringWheel;
	public CarEngine engine;
	private float _torque = 0;
	public GameObject Driver;
	public float dragCoeff = 0.35f;
	public float frontalArea = 1.99f;
	private float[] _pedals = new float[3];
	private Rigidbody _rigidBody;

    public AnimationCurve throlleCurve = new AnimationCurve(new Keyframe(0, 0), new Keyframe(1, 1));

    public CarElectricSystem electric;
	public Transmission transmission;

	public float[] Pedals{
		get{ return _pedals;}
	}

    void OnGUI() {
       // var rb = gameObject.GetComponent<Rigidbody>();
        //Label3D.AtWorldPosition(transform.position + Vector3.up * 2, "Velocity: " + rb.velocity.ToStringEx() + "  Km/h: " + rb.velocity.magnitude * 3.6f);
       
    }

	public void SetPedalState(CarPedal pedal, float state){
		_pedals [(int)pedal] = Mathf.Clamp01 (state);
	}

	public float GetPedalState(CarPedal pedal){
		return _pedals [(int)pedal];
	}

	public Rigidbody GetRigidBody(){
		return _rigidBody;
	}

	public float D{
	    get { return 1; } //(Wheels[2].Target.transform.position - Wheels[0].Target.transform.position).magnitude;}
	}

	public float L{
	    get { return 1; } // (Wheels[1].Target.transform.position - Wheels[0].Target.transform.position).magnitude;}
	}

	public static float OuterWheelAngle(float innerDegree, float d, float l){
		float sign = Mathf.Sign (innerDegree);

		float rad = Mathf.Deg2Rad * innerDegree * sign;
		float ctgB = Mathf.Cos (rad) / Mathf.Sin (rad);
		return Arcctg (ctgB + d / l) * Mathf.Rad2Deg * sign;
		//ctgA = ctgB + d/L
	}

	static float Arcctg(float x){
		return Mathf.PI / 2.0f - Mathf.Atan (x);
	}

	// Use this for initialization
	void Start () {
		_rigidBody = gameObject.GetComponent<Rigidbody> ();
		electric.ignition.Attach (this);

		/*foreach (var w in Wheels) {
			w.OnStart();
		}*/

		//SteeringWheel.OnStart ();

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

	}

	public float Torque(){
		return _torque;
	}

	void OnDrawGizmos(){
		//var rb = gameObject.GetComponent<Rigidbody> ();
		//float airDrag = CarMath.Drag(1.25f, rb.velocity.magnitude, dragCoeff, frontalArea);
	
		//var drag = -airDrag * rb.velocity.normalized ;

		//Gizmos.DrawLine(transform.position + Vector3.up, transform.position + Vector3.up + drag);
	}

    float lastSpeed;
    float measureTime;

    public float testSpeed = 100.0f;
	void FixedUpdate(){

		var rb = gameObject.GetComponent<Rigidbody> ();

        var vel = rb.velocity.magnitude * 3.6f; //km/h
        if(lastSpeed == 0 && vel != 0) {
            measureTime = Time.fixedTime;
            Debug.Log("Go go go");

        } else if(lastSpeed < testSpeed && vel >= testSpeed) {
            Debug.LogFormat("Reached {0} Km/h in {1}", testSpeed, (Time.fixedTime - measureTime));
        }
        lastSpeed = vel;

	    chassis.suspension[0].wheel.driveTorque =
	        chassis.suspension[2].wheel.driveTorque = 1000*GetPedalState(CarPedal.Accelerator);

        chassis.suspension[0].wheel.brakeTorque =
          chassis.suspension[2].wheel.brakeTorque = 1000 * GetPedalState(CarPedal.Brake);

        /* if (rb) {
             float airDrag = CarMath.Drag(1.25f, rb.velocity.magnitude, dragCoeff, frontalArea);

             var drag = -airDrag * rb.velocity.normalized;
             rb.AddForce(-airDrag * rb.velocity.normalized);
         }*/

        /*float torqueSpeed = 20.0f;
		if (Input.GetKey (KeyCode.UpArrow)) {
				_torque += torqueSpeed * Time.fixedDeltaTime;

		} else if (Input.GetKey (KeyCode.DownArrow)) {
				_torque -= torqueSpeed * Time.fixedDeltaTime;
		} else {
			_torque *= 0.7f;
		}*/
        //_torque = Mathf.Clamp (_torque, -_maxTorque, _maxTorque);
        //Wheels [0].Collider.motorTorque = _torque;
        //Wheels [2].Collider.motorTorque = _torque;

        chassis.Integrate(Time.fixedDeltaTime, this);

        /*foreach (var w in Wheels) {
			if(w != null){
				var wheelVelocity = rb.GetPointVelocity(w.transform.position);
				Vector3 wheelForce;
				w.Integrate(Time.fixedDeltaTime, wheelVelocity, (rb.mass / 4.0f * 9.81f), out wheelForce);
				rb.AddForceAtPosition(wheelForce, w.transform.position);
			}
		}*/
	}


}