import lejos.hardware.port.SensorPort;
import lejos.hardware.sensor.EV3GyroSensor;
import lejos.robotics.SampleProvider;

public class GyroDrift {

	private final static int SAMPLE_AMOUNT = 10;
	private final static int TIME_INTERVAL = 20;
	private static int offset = 0;
	private static EV3GyroSensor gyro;

	public static void CalibrateOffset() {
		SampleProvider getrate = gyro.getRateMode();

		float[] level = new float[10];
		int avg=0;

		for (int i=0; i<SAMPLE_AMOUNT; i++) {
			getrate.fetchSample(level, i);
		}
		for (int i=0; i<SAMPLE_AMOUNT; i++) {
			avg+=level[i];
		}
		offset = avg/SAMPLE_AMOUNT;
	}


	public static int getCorrectAngle() {
		float[] level = new float[0];
		SampleProvider getrate = gyro.getRateMode();
		getrate.fetchSample(level, 1);

		int rate = (int) level[0] - offset;
		int angle = rate*TIME_INTERVAL;
		return angle;
	}

	public GyroDrift() {
		gyro = new EV3GyroSensor(SensorPort.S1);
	}
}