using UnityEngine;
using System.Collections;
using System;

[Serializable]
public class PID {
	public bool enabled = true;
	public float kp = 1.0f;
	public float ki = 1.0f;
	public float kd = 1.0f;

	public float outMin = -0.1f;
	public float outMax = 0.1f;

	private float _errorIntegral;
	private float _lastError;
	private float _setpoint;

	public float LastError {get {return _lastError;}}

	private float _p;
	public float P {get {return _p;}}
	private float _i;
	public float I {get {return _i;}}
	private float _d;
	public float D {get {return _d;}}

	public float Setpoint{
		get{ return _setpoint; }
		set{ _setpoint = value; }
	}

	public float Process(float dt, float pv){
		if (!enabled) {
			return 0;
		}
		float error = Setpoint - pv;
		float derivative = (error - _lastError) / dt;

		//derivative = Mathf.Clamp(derivative, outMin, outMax);

		/*if (Mathf.Abs(integralChange) > 0.001f) {
			integralChange = 0.001f * Mathf.Sign(integralChange);
		}*/
		_errorIntegral += error * dt;


		//_errorIntegral = Mathf.Clamp(_errorIntegral, outMin, outMax);

		_lastError = error;

		_p = kp * error;
		_i = ki * _errorIntegral;
		_d = kd * derivative;

		var output = _p + _i + _d;
		return Mathf.Clamp(output, outMin, outMax);
	}
}