/****************************************************
* control.java
*
* Overall control of robot.
* Contains main method, measuring and next spot
* algorithms.
*
* Chris Miller
* godal83@gmail.com
*
*
*****************************************************/

import com.ridgesoft.io.Display;
import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.intellibrain.IntelliBrainDigitalIO;
import com.ridgesoft.robotics.PushButton;
import com.ridgesoft.robotics.Motor;
import com.ridgesoft.robotics.SonarRangeFinder;
import com.ridgesoft.robotics.sensors.ParallaxPing;
import java.lang.Math;


public class control
{
	private static final int RIGHT = 0;
	private static final int LEFT = 1;

	static Display display;
	static SonarRangeFinder sonarSensor;

	static float x = 0,y = 0, prev1 = 0, prev2 = 0;


    public static void main(String args[])
	{
		setup();
		move mover = new move();

		while(true)
		{
			measure(mover);
			next(mover);
		}
    }

	//setup the components on the robot
	public static void setup()
	{
		//display was used for testing but no reason to remove it
		display = IntelliBrain.getLcdDisplay();
		sonarSensor = new ParallaxPing(IntelliBrain.getDigitalIO(3));
	}

	//sits in 1 spot and measures approximately every 45 degrees
	public static void measure(move mover)
	{
		float range;
		int angle;
		for(int i = 0;i < 8;i++)
		{
			angle = mover.getCurrentAngle();
			range = getRange();

			//This should be changed to some kind of file output at some point
			System.out.println(x + "  " + y + "  " + angle + "  " + range);
			prev2 = prev1;
			prev1 = range;
			mover.turn(45);
		}
	}

	//uses previous measurements to make sure it wont run into a wall
	//currently only uses prev1 but prev2 is ready if needed
	public static void next(move mover)
	{
		float range, forwardDistance;
		range = getRange();
		while(range < 15 && prev1 < 15)
		{
			mover.turn(45);
			prev2 = prev1;
			prev1 = range;
			range = getRange();
		}
		forwardDistance = mover.forward(12);

		//determine new x,y coordinates after movement based on angle and distancce traveled
		x += forwardDistance * Math.cos(((((float)(0-mover.getCurrentAngle()))/360)*(2*Math.PI)));
		y += forwardDistance * Math.sin(((((float)(0-mover.getCurrentAngle()))/360)*(2*Math.PI)));
	}

	//use the radar range finder to get measurement
	public static float getRange()
	{
		float range;

		try
		{
			sonarSensor.ping();
			Thread.sleep(2000);
		}
		catch(Exception e)
		{
			System.out.println(e);
		}
		range = (float)(sonarSensor.getDistanceInches()+3.5);
		display.print(0,Float.toString(range));
		return range;
	}
}