using UnityEngine;
using System.Collections;

public class PropellerForce : MonoBehaviour {
	public Motor motor;
	public float maxForce = 20;
	public float maxTorque = 20;
	private Vector3 forceVector;
	private Rigidbody rb;
	private Vector3 torque;

	private Vector3 torqueForce;
	private Vector3 torqueOrtho;

	// Use this for initialization
	void Start () {
		rb = gameObject.GetComponentInParent<Rigidbody> ();
	}

	void FixedUpdate(){
		if (rb == null) {
			return;
		}
		float motorSpeedNormalized = (motor.speed / motor.maxRpm);

		float force = Mathf.Abs(motorSpeedNormalized * maxForce);
		forceVector = motor.transform.up * force;
		rb.AddForceAtPosition(forceVector, motor.transform.position);

		float torqueMagnitude = maxTorque * motorSpeedNormalized;
		torque = -torqueMagnitude * motor.transform.up;

		rb.AddTorqueAtPosition (torque, motor.transform.position, ref torqueForce, ref torqueOrtho);
	}

	void OnDrawGizmos(){
		//return;
		Gizmos.DrawLine (motor.transform.position, motor.transform.position + forceVector);
		Gizmos.color = Color.green;

		var pa = motor.transform.position;
		var pb = motor.transform.position + torqueOrtho;
		var pc = motor.transform.position + torqueOrtho + torqueForce;
		var pd = motor.transform.position - torqueOrtho;
		var pe = motor.transform.position - torqueOrtho - torqueForce;

		Gizmos.DrawLine (pa, pb);
		Gizmos.DrawLine (pb, pc);

		Gizmos.DrawLine (pa, pd);
		Gizmos.DrawLine (pd, pe);
	}

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