import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.robotics.Motor;
import com.ridgesoft.robotics.AnalogShaftEncoder;
import com.ridgesoft.robotics.PIDController;
import com.ridgesoft.io.Display;

public class GoCrazy implements Runnable{

	private Motor mLeftMotor;
	private Motor mRightMotor;
	private AnalogShaftEncoder mLeftEncoder;
	private AnalogShaftEncoder mRightEncoder;
	private int mDuration;
	private float mPGain;
	private float mIGain;
	private float mDGain;
	private final int MPOWER; 
	public int leftPower;
	public int rightPower;

	public GoCrazy(Motor leftMotor, 
					 Motor rightMotor, 
					 AnalogShaftEncoder leftEncoder, 
					 AnalogShaftEncoder rightEncoder,
					 int duration){

		mLeftMotor = leftMotor;
		mRightMotor = rightMotor;
		mLeftEncoder = leftEncoder;
		mRightEncoder = rightEncoder;
		mDuration = duration;
		mPGain = 4.4f;
		mIGain = 20.1f;
		mDGain = 0.0f;
		MPOWER = 10;
		leftPower = MPOWER;
		rightPower = MPOWER;

	}

	public void run(){

		try{

			int errorLeft;
			int errorRight;
			float target = 0.0f;
			float controlLeft;
			float controlRight;
			Display display = IntelliBrain.getLcdDisplay();

			mLeftMotor.setPower(MPOWER);
			if (mLeftEncoder != null)
				mLeftEncoder.updateDirection(true);

			mRightMotor.setPower(MPOWER);
			if (mRightEncoder != null)
				mRightEncoder.updateDirection(true);

			PIDController controller = new PIDController(mPGain, mIGain, mDGain, 2000, false);

			while(true){

				int leftCounts = mLeftEncoder.getCounts();
				int rightCounts = mRightEncoder.getCounts();

				errorRight = rightCounts - leftCounts;
				errorLeft = leftCounts - rightCounts;

				controlLeft = controller.control(target, (float) errorLeft);
				controlRight = controller.control(target, (float) errorRight);
				
				leftPower = MPOWER + (int) controlLeft;
				rightPower = MPOWER + (int) controlRight;

				display.print(0, "LE: " + leftCounts + " EL: " + errorLeft);
				display.print(1, "RE: " + rightCounts + " ER: " + errorRight);

				mLeftMotor.setPower(leftPower);
				if (leftPower<0){
 					mLeftEncoder.updateDirection(false);
				}else{
					mLeftEncoder.updateDirection(true);
				}

				mRightMotor.setPower(rightPower);
				if (rightPower<0){
 					mRightEncoder.updateDirection(false);
				}else{
					mRightEncoder.updateDirection(true);
				}

			}
		}
		catch (Throwable t){
			t.printStackTrace();
		}
	}

	public String toString() {
		return "Go Crazy";
	}

	public int getLeftPower(){
			
		return leftPower;

	}

	public int getRightPower(){
			
		return rightPower;

	}

}