//PID controller and usage in unity c#
//Project: Antigraviator
//http://antigraviator.com
//http://robert-lindner.com/blog/antigraviator/
//Author: Robert Lindenr

//**********
//The controller
//**********
public class PID
{
    private float _seekP = 0;
    private float _p = 0;
    private float _i = 0;
    private float _d = 0;
    private float _oldT = -1;
    private float _oldInput = 0;
    private float _kP = 0;
    private float _kI = 0;
    private float _kD = 0;
    private float _cMin = 0;
    private float _cMax = 0;

    public PID(float kP, float kI, float kD, float cMin, float cMax)
    {
        _kP = kP;
        _kI = kI;
        _kD = kD;
        _cMin = cMin;
        _cMax = cMax;
    }

    public float Seek(float seekVal, float curVal)
    {
        float p = seekVal - curVal;
        float i = _i;
        float d = _d;
        float newInput = _oldInput;
        
        float dT = Time.fixedDeltaTime;

        if(!(_oldT < 0))
        {
            if(dT>0)
            {
                d = (p - _p) / dT;
                float onlyPd = _kP * p + _kD * d;
                if((_i>0 || onlyPd > _cMin)&&(_i<0 || onlyPd < _cMax))
                {
                    i = _i + p * dT;
                }
                newInput = onlyPd + _kI * i;
            }
        }
        newInput = Mathf.Max(_cMin, Mathf.Min(_cMax, newInput));
        _seekP = seekVal;
        _p = p;
        _i = i;
        _d = d;
        _oldT = Time.realtimeSinceStartup;
        _oldInput = newInput;

        return newInput;
    }
}

//*****************
//Example usage
//*****************
public class VehicleBehaviour : MonoBehaviour
{
	//[...]
	
    //PID Tuning Parameters
    [Header("PID Tuning Parameters")]
    [SerializeField]
    private Vector3 HoverTuning = new Vector3(0.7998f, 0.0002f, 0.2f);
    private PID _hoverPID;
    [SerializeField]
    private Vector3 VelTuning = new Vector3(0.5f, 0.0002f, 0.3f);
    [SerializeField]
    private Vector3 TorqueTuning = new Vector3(0.5f, 0.0002f, 0.1f);
    [SerializeField]
    private Vector3 YawTuning = new Vector3(0.5f, 0.0002f, 0.1f);
    private PID _yawPID;
    private PID _velXPID;
    private PID _velYPID;
	
	//[...]
	
    void Hover(float horInput, float vertInput, float strafePerc, float enginePerc)
    {
		
	//[...]
		
        //Strafing
        float xVel = Vector3.Dot(transform.right, _rigBod.velocity);
        float targetVelX = ((horInput * strafePerc)+(_strafeOff*WallStrafeMult))* StrafeVelocity;
        float tXperc = _velXPID.Seek(targetVelX, xVel);
        Vector3 horF = transform.right; horF.y = 0;
        _rigBod.AddForce(horF.normalized * tXperc * _accHor, ForceMode.Acceleration);
		
	//[...]
	
	}
	
	//[...]
}