/**********************************************
* move.java
*
* Controls movement of robot
*
* Chris Miller
* godal83@gmail.com
*
*
***********************************************/

import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.robotics.Servo;
import com.ridgesoft.robotics.ContinuousRotationServo;
import com.ridgesoft.robotics.AnalogInput;
import com.ridgesoft.robotics.ShaftEncoder;

public class move extends Thread
{
	/*****************************************
	* These are the numbers that would change
	* depending on the robot. Degrees/count and
	* inches/count are only ones that are important
	* if you continue to use encoder.
	******************************************/
	private final double DEGREES_PER_SECOND = 15.11;
	private final double INCHES_PER_SECOND = 3.5;
	private final double DEGREES_PER_COUNT_DIFFERENCE = 6.35;
	private final double INCHES_PER_COUNT = .525;


	Servo leftServo;
	Servo rightServo;
	AnalogInput leftInput;
	AnalogInput rightInput;
	int angle;
	int leftCount,rightCount;
	int lowThreshold, highThreshold;
	boolean leftHigh, rightHigh;

	public move()
	{
		//setup servos and encoders
		leftServo = IntelliBrain.getServo(1);
		rightServo = IntelliBrain.getServo(2);
		leftInput = IntelliBrain.getAnalogInput(4);
		rightInput = IntelliBrain.getAnalogInput(5);
		leftCount = 0;
		rightCount = 0;
		lowThreshold = 250;
		highThreshold = 750;

		//angle starts in whatever direction you point robot at start
		angle = 0;

		//find out if encoder is open or blocked
		if(leftInput.sample() < 250)
		{
			leftHigh = false;
		}
		else
		{
			leftHigh = true;
		}

		if(rightInput.sample() < 250)
		{
			rightHigh = false;
		}
		else
		{
			rightHigh = true;
		}
	}


	public void turn(int inDegrees)
	{
		//determine how much time to spend turning based on parameter
		int time = (int)((inDegrees/DEGREES_PER_SECOND)*1000);
		long nextTime = System.currentTimeMillis();
		long stopTime = nextTime + time;

		//50 is off so 51/49 are slowest speeds for servos
		leftServo.setPosition(51);
		rightServo.setPosition(51);

		//turn for determined time checking the encoders every 5ms (could wait longer with this slow of turn if needed)
		try
		{
			while(nextTime < stopTime)
			{
				nextTime += 5;
				try
				{
					checkEncoder(true,false);
					Thread.sleep(nextTime - System.currentTimeMillis());
				}
				catch (InterruptedException e) {}
			}

			leftServo.off();
			rightServo.off();

		}
		catch(Exception e)
		{
			System.out.println("Error in move.java, turn method");
		}

		//determine the new angle based on encoder readings
		angle = ((int)((leftCount - rightCount) * (DEGREES_PER_COUNT_DIFFERENCE)))%360;
	}

	//move forward input # of inches
	public float forward(int inInches)
	{
		//determine time needed to move forward input # of inches
		int time = (int) ((inInches/INCHES_PER_SECOND)*1000);
		long nextTime = System.currentTimeMillis();
		long stopTime = nextTime + time;

		int startRight = rightCount;
		int startLeft = leftCount;

		//used this because lower numbers weren't keeping robot straight
		leftServo.setPosition(54);
		rightServo.setPosition(46);

		//move forward checking encoder every 5ms
		try
		{
			while(nextTime < stopTime)
			{
				nextTime += 5;
				try
				{
					checkEncoder(true,true);
					Thread.sleep(nextTime - System.currentTimeMillis());
				}
				catch (InterruptedException e) {}
			}
			leftServo.off();
			rightServo.off();
		}
		catch(Exception e)
		{
			System.out.println("Error in move.java, forward method");
		}

		//determine new angle robots at in case it didnt move perfectly straight
		angle = ((int)((leftCount - rightCount) * (DEGREES_PER_COUNT_DIFFERENCE)))%360;

		//return the distance traveled
		return ((float)(((((float)(rightCount - startRight))+((float)(leftCount - startLeft)))/2)*INCHES_PER_COUNT));
	}

	public int getCurrentAngle()
	{
		return angle;
	}


	/***********************************************
	* If the encoder was low last time it checks to
	* see if its high now and vice versa. If it has
	* changed it uses parameters to know if it should
	* increase or decrease count.
	************************************************/
	public void checkEncoder(boolean leftForward, boolean rightForward)
	{
		int rightValue = rightInput.sample();
		int leftValue = leftInput.sample();

		if (leftHigh)
		{
			if (leftValue < lowThreshold)
			{
				if (leftForward)
				{
					leftCount++;
				}
				else
				{
					leftCount--;
				}

				leftHigh = false;
			}
		}
		else
		{
			if (leftValue > highThreshold)
			{
				if (leftForward)
				{
					leftCount++;
				}
				else
				{
					leftCount--;
				}

				leftHigh = true;
			}
		}

		if (rightHigh)
		{
			if (rightValue < lowThreshold)
			{
				if (rightForward)
				{
					rightCount++;
				}
				else
				{
					rightCount--;
				}

				rightHigh = false;
			}
		}
		else
		{
			if (rightValue > highThreshold)
			{
				if (rightForward)
				{
					rightCount++;
				}
				else
				{
					rightCount--;
				}

				rightHigh = true;
			}
		}
	}
}