import java.io.IOException;
import lejos.nxt.*;
import lejos.nxt.comm.*;
import java.io.*;
import javax.bluetooth.*;

// Author: ¶¯Á¦ÀÏÄк¢
// Site: http://www.diy-robots.com
// Date: 2010/01/15
// Location: Beijing

public class RubikSolverV2 {
	//Define Sensors
	static UltrasonicSensor distance=new UltrasonicSensor(SensorPort.S1);
	static LightSensor light = new LightSensor(SensorPort.S2);
	static ColorSensor color = new ColorSensor(SensorPort.S3);
	
	//Define Motors
	static Motor paw=Motor.A;
	static Motor bottom=Motor.C;
	static Motor monitor=Motor.B;

	//if the paw has the 3:1 reducer, set it as true 
	static boolean HasReducer = true;
	//the motor angle for paw to hold the cube
	static int PawHoldPosition = 56;
	//the motor angle for paw to rotate the cube
	static int PawTurnOverPosition = 110;
	//the motor angle for base to rotate 90 degree
	static int BaseOneQuarter = 315;
	//the fix angle for base
	static int BaseRotateFix = 40;
	
    public static void main (String[] arg) throws Exception
    {
    	light.setFloodlight(false);
    	bottom.setSpeed(1000);
    	
    	while(true)
    	{
    		Button.waitForPress();
    		int nButtonNum = Button.readButtons();
    		// 1:Enter; 2:Left; 3:Right; 4:Escape
    		switch(nButtonNum)
    		{
    		case 1:
    			Robot.RotatePaw();
    			break;
    		case 2:
    			Robot.RotateBottom(1);
    			break;
    		case 4:
    			Robot.RotateBottomSide(-1);
    			break;
    		default:
    			return;
    		}
    	}
    }
 
    public static class Robot
    {
		public static void RotateBottomSide(int nQuarter) throws Exception 
		{
			paw.setSpeed(400);
			int nFixAngle = BaseRotateFix * ( nQuarter > 0 ? 1 : -1);
			int nPawHoldPosition = PawHoldPosition;
			if(HasReducer) nPawHoldPosition = -3 * nPawHoldPosition;
			paw.rotateTo(nPawHoldPosition);
			bottom.rotate(nQuarter * BaseOneQuarter + nFixAngle);
			bottom.rotate(-nFixAngle);
			paw.rotateTo(0);
		}
		
		public static void RotateBottom(int nQuarter)throws Exception 
		{
			bottom.rotate(nQuarter * BaseOneQuarter);
		}
		
		public static void RotatePaw()throws Exception 
		{
			int nPawHoldPosition = PawHoldPosition - 8;
			if(HasReducer) nPawHoldPosition = -3 * nPawHoldPosition;
			
			int nPawTurnOverPosition = PawTurnOverPosition;
			if(HasReducer) nPawTurnOverPosition = -3 * nPawTurnOverPosition;
			
			paw.setSpeed(1000);
			paw.rotateTo(nPawHoldPosition);
			paw.setSpeed(300);
			paw.rotateTo(nPawTurnOverPosition);
			paw.setSpeed(400);
			paw.rotateTo(nPawHoldPosition);
			paw.setSpeed(1000);
			paw.rotateTo(0);
		}
    }
}