using UnityEngine;

public class PID : MonoBehaviour
{
	public float maxForce;

	public float proportionalGain;

	public float differentialGain;

	private Vector3 lastError = Vector3.zero;

	private Vector3 currentForce = Vector3.zero;

	public float maxTorque;

	public float proportionalGainTorque;

	public float differentialGainTorque;

	private Vector3 lastErrorTorque = Vector3.zero;

	private Vector3 currentTorque = Vector3.zero;

	private Vector3 lastErrorTorqueUp = Vector3.zero;

	private Vector3 currentTorqueUp = Vector3.zero;

	public float currentMultiplier = 1f;

	private Rigidbody rig;

	private SetInertiaTension inertia;

	public Transform addForcePoint;

	private Transform forcePoint;

	private void Start()
	{
		rig = GetComponent<Rigidbody>();
		inertia = GetComponent<SetInertiaTension>();
	}

	private void Update()
	{
		if ((bool)inertia)
		{
			currentMultiplier = 1f - Mathf.Clamp(inertia.collisionValue, 0f, 0.7f);
		}
	}

	private void FixedUpdate()
	{
	}

	public void DoPID(Vector3 targetPosition, Vector3 targetRotation, Vector3 targetRotationUp, float multiplier = 1f)
	{
		AddForce(targetPosition, currentMultiplier * multiplier);
		if (proportionalGainTorque != 0f)
		{
			AddTorque(targetRotation, currentMultiplier * multiplier);
			AddTorqueUp(targetRotationUp, currentMultiplier * multiplier);
		}
	}

	public void AddForce(Vector3 targetPosition, float multiplier = 1f)
	{
		forcePoint = ((!(addForcePoint == null)) ? addForcePoint : base.transform);
		Vector3 vector = targetPosition - forcePoint.position;
		Vector3 vector2 = (vector - lastError) / Time.fixedDeltaTime;
		lastError = vector;
		currentForce = vector * proportionalGain + vector2 * differentialGain;
		currentForce = Vector3.ClampMagnitude(currentForce, maxForce);
		if ((bool)addForcePoint)
		{
			rig.AddForceAtPosition(currentForce * Time.fixedDeltaTime * 60f * multiplier, addForcePoint.position, ForceMode.Acceleration);
		}
		else
		{
			rig.AddForce(currentForce * Time.fixedDeltaTime * 60f * multiplier, ForceMode.Acceleration);
		}
	}

	public void AddTorque(Vector3 targetRotation, float multiplier = 1f)
	{
		Vector3 vector = Mathf.Clamp(Vector3.Angle(targetRotation, base.transform.forward), 0f, 3f) * Vector3.Cross(targetRotation, base.transform.forward);
		Vector3 vector2 = (vector - lastErrorTorque) / Time.fixedDeltaTime;
		lastErrorTorque = vector;
		currentTorque = vector * proportionalGainTorque + vector2 * differentialGainTorque;
		currentTorque = Vector3.ClampMagnitude(currentTorque, maxTorque);
		rig.AddTorque(-currentTorque * Time.fixedDeltaTime * 60f * multiplier, ForceMode.Acceleration);
	}

	public void AddTorqueUp(Vector3 targetRotation, float multiplier = 1f)
	{
		Vector3 vector = Mathf.Clamp(Vector3.Angle(targetRotation, base.transform.up), 0f, 3f) * Vector3.Cross(targetRotation, base.transform.up);
		Vector3 vector2 = (vector - lastErrorTorqueUp) / Time.fixedDeltaTime;
		lastErrorTorqueUp = vector;
		currentTorqueUp = vector * proportionalGainTorque + vector2 * differentialGainTorque;
		currentTorqueUp = Vector3.ClampMagnitude(currentTorqueUp, maxTorque);
		rig.AddTorque(-currentTorqueUp * Time.fixedDeltaTime * 60f * multiplier, ForceMode.Acceleration);
	}
}