public class PhysicsBody : MonoBehaviour {
    [Header( "Calculated Variables" )]
    public Vector3 CenterOfMass;
    public double TotalMass;
    public double MomentOfInertia;
    public Vector3 CurrentVelocity;
    public double CurrentTime;
    public Vector3 InitialVelocity;
	public Vector3 CurrentAngularVelocity;
	public Vector3 CurrentAngularAcceleration;
	public Vector3 AngularDisplacement = Vector3.zero;
    public float Timer = 0f;
	public Vector2 PointPosition;
	public Vector2 PointVelocity;
	//Flipped signs for display
	public Vector2 PointAcceleration;
	public float Theta;
	public float Omega;
	public float Alpha;
    public float Gamma;
	public Vector2 Radial = Vector2.zero;

    [Header( "Preset Variables" )]
    public bool Gravity = false;
    public float Drag = 0.0f; // N/m/s
	public float WindMagnitude = 0.0f; // m/s
	public bool WindFacingObject = true;
	public Vector3 WindDirection = Vector3.zero;
    public float Cw = 0.1f; // N/m/s
    public float AccelerationTime = 8;
    public float ConstantAcceleration = -10; //m/s^2
    public float InitialVelocitySpeed = 100f; //ms/s
    public Vector3 InitialVelocityAngles = new Vector3( 0, 0, 0 ); //angles
	public Vector3 InitialAngularVelocity = new Vector3(0, 0, 1.8f); //angular velocity, radians per second
	public Vector3 InitialAngularAcceleration = new Vector3(0, 0, 0);

    [Header( "Controls" )]
    [InspectorButton( "StartToAcceleration" )]
    public bool Accelerate;
    [InspectorButton( "ResetPhysics" )]
    public bool Reset;

    private bool m_currentlyMoving = false;
    private Vector3 m_startPosition;
	private Transform m_reference;

    // Use this for initialization
    void Start() {
        UpdateValues();
        m_startPosition = transform.position;
		var reference = new GameObject ();
		reference.name = "Physics";
		m_reference = reference.transform;
    }

    PhysicsProperties[] GetPhysicsProperties() {
        return GetComponentsInChildren<PhysicsProperties>();
    }

    double GetTotalMass() {
        var totalMass = 0.0;
        foreach ( var child in GetPhysicsProperties() ) {
            totalMass += child.Mass;
        }
        return totalMass;
    }

    Vector3 GetCenterOfMass() {
        var xCenterMass = 0.0;
        var yCenterMass = 0.0;
        var zCenterMass = 0.0;
        foreach ( var child in GetPhysicsProperties() ) {
            var childCenterPoint = child.GetCenterPoint();
            xCenterMass += child.Mass * childCenterPoint.x;
            yCenterMass += child.Mass * childCenterPoint.y;
            zCenterMass += child.Mass * childCenterPoint.z;
        }
        xCenterMass /= TotalMass;
        yCenterMass /= TotalMass;
        zCenterMass /= TotalMass;
        return new Vector3( (float)xCenterMass, (float)yCenterMass, (float)zCenterMass );
    }

    void CalculateDistanceFromCenterOfMassInChildren() {
        foreach ( var child in GetPhysicsProperties() ) {
            child.GetDistanceFromCenterOfMass( CenterOfMass );
        }
    }

    double GetTotalMomentOfInertia() {
        var inertia = 0.0;
        foreach ( var child in GetPhysicsProperties() ) {
            inertia += child.GetMomentOfIntertiaTheCOM( CenterOfMass );
        }
        return inertia;
    }

    public void UpdateValues() {
        TotalMass = GetTotalMass();
        CenterOfMass = GetCenterOfMass();
        CalculateDistanceFromCenterOfMassInChildren();
        MomentOfInertia = GetTotalMomentOfInertia();
    }

    public void UpdatePosition( double seconds ) {
        //Bullet through air with air resistance
        if (!m_gunStop)
        {
            //Old way, used if Drag is set to 0
            if (Drag == 0)
            {
                //In the same way we had to rotate 90 degrees to face the target, we rotate on that same y axis to point in its direction, not the z axis

                var deceleration = new Vector3(0, -9.81f, 0);

                //x/y/z velocity
                transform.position = transform.position + (float)seconds * CurrentVelocity;
                CurrentVelocity += deceleration * (float)seconds;

                //angular velocity
                var degreesToRotate = -CurrentAngularVelocity * (float)seconds * Mathf.Rad2Deg;

                transform.rotation *= Quaternion.Euler(degreesToRotate);
                AngularDisplacement += CurrentAngularVelocity * (float)seconds;
                CurrentAngularVelocity = CurrentAngularVelocity + CurrentAngularAcceleration * (float)seconds;

                //points angular stuff
                var currentAngularAccelerationShouldBeWhyDidIInvertTheFuckingPlanes = new Vector3(CurrentAngularAcceleration.z, CurrentAngularAcceleration.y, CurrentAngularAcceleration.x);
                var currentAngularVelicityShouldBe = new Vector3(CurrentAngularVelocity.z, CurrentAngularVelocity.y, CurrentAngularVelocity.x);
                if (transform.childCount > 0)
                {
                    var point = transform.GetChild(0);
                    var R = point.transform.position - transform.position;
                    print(R);
                    PointPosition = point.transform.position;
                    PointVelocity = CurrentVelocity + (Vector3.Cross(currentAngularVelicityShouldBe, R));
                    var pointAcceleration = new Vector3(0.0f, -9.81f, 0.0f) + (Vector3.Cross(currentAngularAccelerationShouldBeWhyDidIInvertTheFuckingPlanes, R)) + (Vector3.Cross(currentAngularVelicityShouldBe, Vector3.Cross(currentAngularVelicityShouldBe, R)));
                    PointAcceleration = new Vector3(pointAcceleration.x, pointAcceleration.y, pointAcceleration.z);
                }
            // Project 08 code
            }
            else {
                var m = GetTotalMass();
                var xo = m_startPosition.x;
                var vxo = InitialVelocity.x;
                var t = Timer;
                var Cd = Drag;
                var Tau = (float)m / Cd;
                var w = WindMagnitude;
                var g = -9.81f;
                var yo = m_startPosition.y;
                var vyo = InitialVelocity.y;
                var zo = m_startPosition.z;
                var vzo = InitialVelocity.z;
                var gamma = Gamma;

                //Tau * (1-E^(-tTau))
                var TauE = Tau * (1 - Mathf.Exp(-t / Tau));
                //Cw * w * Cos(y) / Cd
                var CwWCosCd = ((Cw * w * Mathf.Cos(gamma)) / Cd);
                //Cw * w * Sin(y) / Cd
                var CwWSinCd = ((Cw * w * Mathf.Sin(gamma)) / Cd);

                var x = xo + (vxo * TauE) + (CwWCosCd * Tau * TauE) - (CwWCosCd * t);
                var y = yo + (vyo * TauE) + (g * Tau * Tau * TauE) - (g * Tau * t);
                var z = zo + (vzo * TauE) + (CwWSinCd * Tau * TauE) - (CwWSinCd * t);

                transform.position = new Vector3(x, -y, z);
            }
            Timer += (float)seconds;
        }

        if (transform.position.y <= 0)
        {
            m_currentlyMoving = false;
        }
    }

    public void StartToAcceleration() {
		var gunAngleToHit = GameObject.Find ("Gun").GetComponent<PhysicsTargetting> ().GunAngleToHit;
		InitialVelocityAngles = new Vector3 (gunAngleToHit.x, gunAngleToHit.y, 0.0f);
        m_startPosition = transform.position;
        m_currentlyMoving = true;
        CurrentTime = 0;
		m_reference.eulerAngles = new Vector3( InitialVelocityAngles.x, (-InitialVelocityAngles.y) + 90 - InitialVelocityAngles.z, m_reference.eulerAngles.z );
        var initialVelocityDirection = m_reference.forward;
        InitialVelocity = InitialVelocitySpeed * initialVelocityDirection;
        CurrentVelocity = InitialVelocity;
		CurrentAngularVelocity = InitialAngularVelocity;
		CurrentAngularAcceleration = InitialAngularAcceleration;
        Timer = 0f;
    }

    public void ResetPhysics() {
        transform.position = m_startPosition;
        CurrentTime = 0;
        m_currentlyMoving = false;
        InitialVelocity = Vector3.zero;
        CurrentVelocity = Vector3.zero;
        m_gunStop = false;
        var trail = GetComponent<TrailRenderer>();

        if ( trail != null ) {
            trail.Clear();
        }
    }

	void FixedUpdate() {
		CenterOfMass = GetCenterOfMass ();
        if ( m_currentlyMoving ) {
            UpdatePosition( Time.deltaTime );
        }
    }

    private bool m_gunStop = false;
    void OnTriggerEnter( Collider other ) {
        if ( other.gameObject.tag == "Immobile" ) {
            CurrentVelocity = Vector3.zero;
            m_gunStop = true;
        }
    }

	public void ApplyForce(Vector3 atPoint, Vector3 direction, float newtons, float deltaTime) {
		if (!m_currentlyMoving) {
			StartToAcceleration ();
			m_gunStop = true;
		}
		print ("ApplyingForce Called");
		var F = newtons;// * direction; //Only on one direction s
		var R = Vector2.Distance(new Vector2(CenterOfMass.x, CenterOfMass.z), new Vector2(atPoint.x, atPoint.z));//CenterOfMass - (transform.position + atPoint);
		var forceDirectionRaw = atPoint + direction;
		var forceDirection2D = new Vector2 (forceDirectionRaw.x, forceDirectionRaw.z);
		var COMRaw = CenterOfMass - atPoint;
		var COM2D = new Vector2 (COMRaw.x, COMRaw.z);
		var T = 180f - Vector3.Angle (forceDirection2D, COM2D); // Angle between force direction and direction to COM
		//print((CenterOfMass - atPoint) + " " + direction);
		//print(T);
		//print (T + " " + (transform.position + direction.normalized) + " " + (transform.position + (CenterOfMass - transform.position).normalized));
		var NewT = Mathf.Abs(CenterOfMass.z - atPoint.z)/R;
		var angularVelocityBeforeDivision = new Vector3(0.0f, R * F * NewT, 0.0f);//* Mathf.Sin (T);
		var angularAcceleration = angularVelocityBeforeDivision / (float)MomentOfInertia;
		var accelerationMagnitude = newtons / TotalMass;
		var acceleration = direction * (float)accelerationMagnitude * deltaTime; // m/s
		//print(deltaTime);

		var degreesToRotate = -CurrentAngularVelocity * deltaTime * Mathf.Rad2Deg;
		//transform.rotation *= Quaternion.Euler (degreesToRotate);
		transform.RotateAround(CenterOfMass, Vector3.up, degreesToRotate.y);

		AngularDisplacement += CurrentAngularVelocity * deltaTime;
		Theta = AngularDisplacement.y;
		Omega = CurrentAngularVelocity.y;
		Alpha = CurrentAngularAcceleration.y;
		CurrentAngularAcceleration = angularAcceleration;
		CurrentAngularVelocity = CurrentAngularVelocity + CurrentAngularAcceleration * deltaTime;

		print(string.Format("Acceleration: {0}", direction * (float)accelerationMagnitude));
		CurrentVelocity += acceleration;
	}
}