package org.termi;
import lejos.nxt.*;
import lejos.util.*;
import lejos.robotics.navigation.*;
public class Brain {
	private static double kp = 10;
	private static double kd = 2;
	private static double ki = 2;
	private static double speed = 100; // 0 - 100
	private static double soll = 0;//null abweichung, wenn notwendig ändern

	private static double m_p;
	private static double m_i;

	private static long abtastRate = 2000; // in ms

	private static int ERROR_ROW = 0;
	private static int Y_ROW = 1;
	private static double ealt = 0;
	private static double esum = 0;
	private static double error()
	{
		LightSensor s1 = new LightSensor(SensorPort.S2);//links
		LightSensor s2 = new LightSensor(SensorPort.S3);//rechts

		int links = s1.readNormalizedValue();
		int rechts = s2.readNormalizedValue();
		int error = -links + rechts;
		LCD.drawString(String.valueOf(error), 0, ERROR_ROW);
		return error;
	}
	private static void move()
	{
		double e = error();
		esum = esum + e;
		double y = kp * e + ki * abtastRate * esum  + kd * (e-ealt)/abtastRate;
		LCD.drawString(String.valueOf(y), 0, Y_ROW);
		ealt = e;

		//move
	}

    public static void main (String[] args) {
		System.out.println("Hello World");
		Pilot pilot = new TachoPilot(2.1f, 4.4f, Motor.B, Motor.C, true);
		SimpleNavigator tn = new SimpleNavigator(pilot);
		for(int i = 0; i < 2; i++) {
			tn.forward();
			Delay.msDelay(1000);
			tn.stop();

			double e = error();
			esum = esum + e;
			double y = kp * e + ki * abtastRate * esum  + kd * (e-ealt)/abtastRate;
			LCD.drawString(String.valueOf(y), 0, Y_ROW);
			ealt = e;

		}
		for(int i = 0; i < 2; i++) {
			tn.backward();
			Delay.msDelay(1000);
			tn.stop();

			double e = error();
			esum = esum + e;
			double y = kp * e + ki * abtastRate * esum  + kd * (e-ealt)/abtastRate;
			LCD.drawString(String.valueOf(y), 0, Y_ROW);
			ealt = e;

		}
		tn.stop();
    }

}