package com.aelitis.azureus.core.util.loopcontrol.impl;

import com.aelitis.azureus.core.util.loopcontrol.LoopControler;

public class PIDLoopControler implements LoopControler {

	double pGain;
	double iGain;
	double dGain;
	
	double iState;
	double iMin = -5000;
	double iMax =  5000;
	
	double dState;
	
	public PIDLoopControler(double pGain,double iGain, double dGain) {
		this.pGain = pGain;
		this.iGain = iGain;
		this.dGain = dGain;
	}
	
	public double updateControler(double error, double position) {
		
		//Proportional
		double pTerm = pGain * error;
		
		
		iState += error;
		if(iState > iMax) iState = iMax;
		if(iState < iMin) iState = iMin;
		
		double iTerm = iGain * iState;
		
		double d = dState - position;
		
		double dTerm = dGain * d;
		dState = position;

		double result = pTerm + iTerm - dTerm;
		
		System.out.println("PID p,i,d (" + pGain + "," + iGain + "," + dGain +") : is,ds (" + iState + "," + d + ") p,i,d (" + pTerm + "," + iTerm + "," + dTerm + ") => " + result);
		
		return result;
	}
	
	public void reset() {
		dState = 0;
		iState = 0;
	}

}