using UnityEngine;
using System.Collections;

public class EngineRod : MonoBehaviour
{
    public Transform anchor;
    public Transform piston;

    public float rodLength;
    public Vector3 upVector;

    private Vector3 startPosition;
    private Quaternion rotation;
    private Quaternion pistonRotation;
    // Use this for initialization
    void Start ()
    {
        upVector = piston.position - anchor.position;

        rodLength = (transform.localPosition - transform.parent.InverseTransformPoint(piston.position)).magnitude;

        upVector.Normalize();
        startPosition = transform.localPosition;
        upVector = transform.parent.InverseTransformVector(upVector);

        rotation = transform.localRotation;
        pistonRotation = piston.localRotation;
    }

    public float projectionDistance;
	// Update is called once per frame
	void Update () {
	    if (anchor != null)
	    {
	        transform.position = anchor.position;

	        Ray r = new Ray(startPosition, upVector);
	        var localPosProjected = r.Project(transform.localPosition);
	        projectionDistance = (localPosProjected - transform.localPosition).magnitude;

	        float pistonOffset = Mathf.Sqrt(rodLength*rodLength - projectionDistance*projectionDistance);

	        float angle = (90.0f -  Mathf.Asin(pistonOffset/rodLength) * Mathf.Rad2Deg) *- Mathf.Sign(Vector3.Dot(transform.localPosition - localPosProjected, Vector3.forward));


	        transform.localRotation = Quaternion.AngleAxis(angle, Vector3.right) * rotation;
       
            piston.localRotation = pistonRotation * Quaternion.AngleAxis(-angle, Vector3.right);
        }
	}
}