// TODO
//
// - limit sendData to x per second
// - errorSequence()
// - insert into calibration files
// - Make sure ENUM piDataType is correct and synced
// - Change interpretData to resemble correct ENUM data
// 

// *****************
// ** ERROR CODES **
// *****************
// 
// 0		no error
// 1		Error: Sensor Compass error
// 2		Error: Sensor Obstacle detection on the turret
// 3		Error: Sensor IR Line detection

#define DEBUG true

#ifdef DEBUG
#define DEBUG_PRINT(str) Serial.print(str)
#else
#define DEBUG_PRINT(str) do { } while (0)
#endif

#include <Servo.h>
#include <HMC5883L.h>
#include <Wire.h>
#include <TrueRandom.h>

// **********************
// ** Calibration file **
// **********************

#include "definitions.h"
#include "dataTypes.h"

// Include Wally or Eve, (un)comment the right one.
#define ROVER "wall-e"
#include "calibration_wall-e.h"
//#include "calibration.eve.h"


// *****************
// ** DEFINITIONS **
// *****************

// Servos
Servo servoLeft;
Servo servoRight;
Servo servoTurret;
Servo servoGripper;

// Compass
HMC5883L compass;

// Data variables
data_t data;
int dataToPiChangedBits = 0;
bool RPiActive = true;
unsigned long lastCommunication = 0;

// Current operation mode
opmode_t operationMode = OPMODE_WAIT;
opmode_t operationModeDefault = OPMODE_INITIALSEQUENCE;
unsigned long operationChange = 0;

// Location values
int currValRobotX = 0;
int currValRobotY = 0;
int LabX = 0;
int LabY = 0;

// Number of directions the turret can measure from
int turretSequence[NUM_TURRET_DIRECTIONS] = {};

// Sensor values
int currValSensCompass = 0;
int currValSensObstacleTurretLow = 0;
int currValServoTurret = 0;

// Actuator values
int currValTurret = 0;
int currValDirection = 0;

// Timers
long timerMovementStart = 0;
long timerMovementStop = 0;
long timerInitialSequence = 0;
long timerTurret = 0;

// Counters
int counterInit = 0;
int counterMovement = 0;
int counterTurret = -1;
int counterTurretWait = 0;


// **********************
// ** HELPER FUNCTIONS **
// **********************

// prototypes
void opMode(opmode_t opmode);
void stopAllServos();

#if 0
// error helper
void error(int errorCode) {
	opMode(OPMODE_ERROR);
	stopAllServos();
	errorSequence(errorCode);
}
void errorSequence(int errorCode) {
	while(1) {
		// - move turret to middle (fast)
		// - move turret to left (slow)
		// - move turret to middle (slow)
		delay(1000);
		for(int i = 0; i < errorCode; i++) {
			// - move turret to right (fast)
			// - move turret to middle (fast)
			delay(500);
		}
		delay(3000);
	}
}
#endif

long microsecondsToInches(long microseconds)
{
	// According to Parallax's datasheet for the PING))), there are
	// 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
	// second).  This gives the distance travelled by the ping, outbound
	// and return, so we divide by 2 to get the distance of the obstacle.
	// See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
	return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds)
{
	// The speed of sound is 340 m/s or 29 microseconds per centimeter.
	// The ping travels out and back, so to find the distance of the
	// object we take half of the distance travelled.
	return microseconds / 29 / 2;
}

void initTurretSequence(int array[], int numOfPositions) {
	int sign = 1;
	int x = 0;

	for (int i = 0; i < numOfPositions; i++) {
		array[i] = x;
		x += sign * 2;

		if(x > numOfPositions) {
			sign = -1;
			x -= 3;
		}
		if(x == numOfPositions) {
			sign = -1;
			x -= 1;
		}
	}
}



// **********************
// ** PI COMMUNICATION **
// **********************

// sendData to Raspberry Pi
void sendData(pi_datatype_t method, int data, int data1, int data2, int data3) {
	if (1 == 1) {
		lastCommunication = millis();
	}
	
}

void interpretData(pi_datatype_t dataType, int message) {
	switch(dataType) {
		case PI_DATATYPE_MOVETO:
		
		break;
		case PI_DATATYPE_GRIPPER:
    
		break;
		case PI_DATATYPE_TURRET:
    
		break;
		default:
		// ignore ????
		break;
	}
}

// receive data from Raspberry Pi
// sequence = ... ?????
void readData() {
  
	// if data received -> interpretate it
	// change operation mode + update timer
	operationMode = OPMODE_WAIT;
	operationChange = millis();
	// for example:
	interpretData(PI_DATATYPE_MOVETO, 0);
	if (1 == 1) {
		lastCommunication = millis();
	}
}

bool checkIfPiAlive() {
	return false; // CONNECT TO PI TO CHECK IF STILL ALIVE
}

bool checkRPiActive() {
	RPiActive = (lastCommunication + MAX_COMMUNICATION_DELAY) > millis() ? 1 : 0;
	if(!RPiActive) {
		RPiActive = checkIfPiAlive();
	}
	return RPiActive;
}

// **********************
// ** SENSOR FUNCTIONS **
// **********************

// sensor Compass, orientation
float sensMagnetometer() {
	MagnetometerRaw raw = compass.ReadRawAxis();
	float heading = atan2(raw.YAxis, raw.XAxis);

	if(heading < 0){
		heading += 2*PI;
	}

	float headingDegrees = heading * 180/M_PI;    // Convert to angles
	headingDegrees = headingDegrees + 90;         // Adjust angle to be relative to front of robot instead of X-direction of sensor
	if(headingDegrees>360){
		headingDegrees=headingDegrees-360;
	}
	float north;
	if(headingDegrees<165){                     // When heading is negative (left) of north with 15 degree margin
		north=headingDegrees-180;                   // Set angle between north and heading angle
	}
	else if(headingDegrees>195){                  // When heading is positive (right) of north with 15 degree margin
		north=headingDegrees-180;                   // Set angle between north and heading angle
	}
	else{                                         // Else heading is towards north with 15 degree margin both sides            
		north=0;                                    // Set angle between north and heading angle to be 0
	}
	return(north);                                // Returns angle difference
}
void sensCompass() {
	int degree = -1;
	float reading[NUM_COMPASS_CHECKS];
	float readingtotal = 0;
	
	for(int i = 0; i < NUM_COMPASS_CHECKS; i++){
		readingtotal += sensMagnetometer();
	}
	
	readingtotal /= NUM_COMPASS_CHECKS;
  
	if (readingtotal >= 0 && readingtotal <= 360)
		degree = (int) readingtotal;
	
	if (data.compass != degree) {
		data.compass = degree;
		// mark as changed
		dataToPiChangedBits |= CHANGED_SENS_COMPASS;
	}
}

// sensor Ultrasound, distance measurement
long sensPing(int pin)
{
	long duration;
	
	// Sets pin to OUTPUT and send start signal: pulse (Low-High-Low for clean signal)
	pinMode(pin, OUTPUT);
	digitalWrite(pin, 0);
	delayMicroseconds(2);
	digitalWrite(pin, 255);
	delayMicroseconds(5);
	digitalWrite(pin, 0);
	
	// Sets pin to INPUT and measure time from the echo
	pinMode(pin, INPUT);
	duration = pulseIn(pin, 255);
	
	// Convert from us to cm
	duration = microsecondsToCentimeters(duration);
	return duration;
}
void sensObstacleTurret() {
	
	// BE AWARE, LOCALISATION OF OBSTACLE IS y DISTANCE FROM CENTER OF ROBOT 
	// (creates triangle with centerOfRobot, centerOfSensor and Obstacle as its corners)
	
	if (timerTurret == 0) {
		long distance = sensPing(PIN_SENS_OBSTACLE_TURRET);
		if (data.obstacle_turret_distances[counterTurret] != distance) {
			data.obstacle_turret_distances[counterTurret] = distance;
			// mark as changed
			dataToPiChangedBits |= CHANGED_SENS_OBSTACLETURRET;
		}
	}
}


// sensor IR, line detection
void sensIRLine(lr_t LorR) {
	
	// BE AWARE, LOCALISATION OF LINE IS y DISTANCE and z DEGREE FROM CENTER OF ROBOT,
	// calculate by RPi??
	
	int i;
	bool inaccessible = false;
	int pinNo = LorR == L ? PIN_SENS_IR_LEFT : PIN_SENS_IR_RIGHT;
	int threshold = LorR == L ? CAL_IR_LEFT_THRESHOLD : CAL_IR_RIGHT_THRESHOLD;
	bool *dataField = LorR == L ? &data.IR_left_detected : &data.IR_right_detected;
	int changeBit = LorR == L ? CHANGED_SENS_IRLEFT : CHANGED_SENS_IRRIGHT;
  
	i = analogRead(pinNo);
	if (i > threshold)
		inaccessible = true;
	
	if (*dataField != inaccessible) {
		*dataField = inaccessible;
		// mark as changed
		dataToPiChangedBits |= changeBit;
	}
}

// sensor IR, sample detection
void sensSampleTurret() {
	
	// BE AWARE, LOCALISATION OF OBSTACLE IS y DISTANCE FROM CENTER OF ROBOT 
	// (creates triangle with centerOfRobot, centerOfSensor and Obstacle as its corners)
	
	if (timerTurret == 0) {
		int i;
		bool sample = false;
		
		i = false; // SENSORDATA HERE
		if (i > CAL_SAMPLE_TURRET_THRESHOLD) 
			sample = true;
		
		if (data.sample_turret_detected[counterTurret] != sample) {
			opMode(OPMODE_WAIT);
			stopAllServos();
			data.sample_turret_detected[counterTurret] = sample;
			// mark as changed
			dataToPiChangedBits |= CHANGED_SENS_SAMPLETURRET;
		}
	}
}
void sensSampleGripper() {
	int i;
	bool sample = false; 
	
	i = false; // SENSORDATA HERE
	if (i > CAL_SAMPLE_GRIPPER_THRESHOLD) 
		sample = true;
	
	if(data.sample_gripper_detected != sample) {
		data.sample_gripper_detected = sample;
		// Mark as changed
		dataToPiChangedBits |= CHANGED_SENS_SAMPLEGRIPPER;
	}
}

// sensor IR, beacon detection and recognition
int sensBeaconTurret() {
	// BE AWARE, LOCALISATION OF OBSTACLE IS y DISTANCE FROM CENTER OF ROBOT 
	// (creates triangle with centerOfRobot, centerOfSensor and Obstacle as its corners)
	
	if (timerTurret == 0) {
		int i;
		bool beacon = false;
		
		i = false; // SENSORDATA HERE
		if (i > CAL_SAMPLE_TURRET_THRESHOLD) 
			beacon = true;
		
		if (data.beacon_detected[counterTurret] != beacon) {
			data.beacon_detected[counterTurret] = beacon;
			// mark as changed
			dataToPiChangedBits |= CHANGED_SENS_SAMPLETURRET;
		}
	}
}

// sensor IR, lab detection
void sensLab() {
	// ...
}

// ************************
// ** ACTUATOR FUNCTIONS **
// ************************

void maneuver(int speedLeft, int speedRight){
  speedLeft *= CAL_SERVO_LEFT;
  speedRight *= CAL_SERVO_RIGHT;
	
  servoLeft.writeMicroseconds(1500 + speedLeft);         // Set left servo speed
  servoRight.writeMicroseconds(1500 - speedRight);       // Set right servo speed
}

void stopMovement() {
	DEBUG_PRINT(" Stop Movement\n");
	maneuver(0, 0);
	timerMovementStart = 0;
	timerMovementStop = 0;
}

void stopAllServos() {
	DEBUG_PRINT(" Stop All Servos\n");
	maneuver(0, 0); 
	timerTurret = 0;
	timerMovementStart = 0;
	timerMovementStop = 0;
}

bool moveTurnTo(int direction) {
	if (direction == 0 || direction == NUM_DIRECTIONS) {
		return true;
	}
	
	// Make sure turret doesn't move
	timerTurret = 1;
	
	// if movementtimer is not started, start timer or something and initiate movement
	// else continue movement and return false
	// if movementtimer is expired, stop movement.
	
	if (!timerMovementStop) {
		DEBUG_PRINT("  Start turn move: ");
		DEBUG_PRINT(direction);
		DEBUG_PRINT(" - ");
		counterMovement++;
		// timer = now() + direction * formula
		timerMovementStart = millis();
		timerMovementStop = millis() + (direction % NUM_DIRECTIONS) * CAL_MOVE_TURN * (360 / NUM_DIRECTIONS);
		
		// DO A COMPASS CHECK
		
		maneuver(0,0);
		if(direction < NUM_DIRECTIONS){
			DEBUG_PRINT(" left\n");
			// turn left
			int sign = -1;
			maneuver(-100,100);
		}
		if(direction >= NUM_DIRECTIONS){
			DEBUG_PRINT(" right\n");
			// Turn right
			int sign = 1;
			maneuver(100,-100);
		}
	} else {
		int deg = ((int)((millis() - timerMovementStart) * CAL_MOVE_TURN * sign)) % 360;
		data.robot_curr_deg = (data.robot_curr_deg + deg + 360*3) % 360;
	}
	
	if (timerMovementStop < millis()) {
		stopMovement();
		currValDirection = 0;
		for (int k = 0; k < NUM_TURRET_DIRECTIONS; k++) {
			data.obstacle_turret_distances[k] = 300;
		}
	}
}

void moveStraight(int direction, float distance) {
	if (direction != 0 && direction != NUM_DIRECTIONS) {
		return;
	}
	
	// Convert distance into drive time
	float drivetime = distance / CAL_MOVE_STRAIGHT;
	
	if (!timerMovementStop) {
		DEBUG_PRINT("  Start straight move: ");
		DEBUG_PRINT(distance);
		DEBUG_PRINT(" - ");
		counterMovement++;
		// timer = now() + direction * formula
		timerMovementStart = millis();
		timerMovementStop = millis() + drivetime;
		
		// If input is positive drive forward
		if(distance > 0) {
			DEBUG_PRINT(" fw\n");
			maneuver(100, 100);
		}
		// If input is negative drive backwards
		if(distance < 0){
			DEBUG_PRINT(" bw\n");
			maneuver(-100, -100);
		}
	} else {
		float dist = (millis() - timerMovementStart) * CAL_MOVE_STRAIGHT;
		data.robot_curr_x += dist * sin((data.robot_curr_deg/180.0)*PI);
		data.robot_curr_y += dist * cos((data.robot_curr_deg/180.0)*PI);
	}
	
	if (timerMovementStop < millis()) {
		stopMovement();
		counterMovement = 0;
	}
	
}

void turnTurretTo(int deg) {
	deg = deg + CAL_TURRET_DEGREE;
	servoTurret.write(deg);
}
void turnTurretToNext() {
	if(timerTurret == 0) {
		counterTurret++;
		counterTurretWait++;
		if(counterTurret >= NUM_TURRET_DIRECTIONS) {
			counterTurret = 0;
		}
		
		int deg;
		deg = turretSequence[counterTurret] * 10;
		
		timerTurret = millis();
		turnTurretTo(deg);
	}
	if ((timerTurret + 100) < millis()) {
		timerTurret = 0;
	}
}


// *********************
// ** LOGIC FUNCTIONS **
// *********************

void opMode(opmode_t opmode) {
	DEBUG_PRINT("\nOPMODE");
	DEBUG_PRINT(opmode);
	DEBUG_PRINT("\n\n");
	operationMode = opmode;
	counterTurretWait = 0;
}

void checkFreePath() {
	// Line detection
	if ((dataToPiChangedBits & (CHANGED_SENS_IRLEFT | CHANGED_SENS_IRRIGHT)) && (data.IR_left_detected || data.IR_right_detected)) {
		// Left or Right IR sensor sees inaccessible terrain
		opMode(OPMODE_WAIT);
		stopMovement();
		DEBUG_PRINT(" NO FREE PATH: left-");
		DEBUG_PRINT(data.IR_left_detected);
		DEBUG_PRINT(" - right-");
		DEBUG_PRINT(data.IR_right_detected);
	}
	
	// Ultrasound
	if (// (dataToPiChangedBits & CHANGED_SENS_OBSTACLETURRET) && 
			(counterTurret >= (NUM_TURRET_DIRECTIONS / 6)) && 
			(counterTurret < (NUM_TURRET_DIRECTIONS - (NUM_TURRET_DIRECTIONS / 6))) &&
			(data.obstacle_turret_distances[counterTurret] < OBSTACLE_TOO_CLOSE)) {
		// Ultrasound sensor sees inaccessible terrain
		opMode(OPMODE_WAIT);
		stopMovement();

		DEBUG_PRINT("   NO FREE PATH: Obstacle turret-");
		DEBUG_PRINT(data.obstacle_turret_distances[counterTurret]);
	}
}

int checkBestRoute(int preferedDirection) {
	// possibly use time instead of boolean values, to remember them, default value = default time, possible to let pi change this
	// possibly change directionArray to global variable
	if (counterMovement) {
		return 0;
	}
	
	// Array with all directions (for example with 12 directions):
	// Bottom half: ([00-11] % 12) * (360/12) degrees to the left
	// Top half: 		([12-23] % 12) * (360/12) degrees to the right
	int directionArray[NUM_DIRECTIONS * 2] = {};
	for (int i = 0; i < (NUM_DIRECTIONS * 2); i++) {
		if (preferedDirection == -1) {
			directionArray[i] = 1;
		} else {
			directionArray[i] = 100 + abs((int)(100 * (float)(abs(((NUM_DIRECTIONS - abs(i - NUM_DIRECTIONS)) % NUM_DIRECTIONS) - preferedDirection) - (float)NUM_DIRECTIONS / 2)));
		}
		
		// limit to first 180deg
		if( (i % NUM_DIRECTIONS) > (NUM_DIRECTIONS / 2) ) {
			directionArray[i] = 0;
		}
	}
	
	if (!eliminateDirections(directionArray)) {
		// We are screwed!!! Basically robot is dead or trapped.
		return false;
		while(1) {
			delay(1000);
		}
	}
	
	int direction = 0;
	int sumDirectionPriority = 0;
	int randValue = 0;
	
	for (int i = 0; i < (NUM_DIRECTIONS * 2); i++) {
		sumDirectionPriority += directionArray[i];
	}
	
	do {
		randValue = TrueRandom.random(sumDirectionPriority);
		for (int i = 0; i < (NUM_DIRECTIONS * 2); i++) {
			randValue -= directionArray[i];
			if (randValue < 0) {
				direction = i--;
				break;
			}
		}
	} while (!directionArray[direction]);
	
	currValDirection = direction;
	return direction;
}

int eliminateDirections(int directionArray[]) {
	int numPossibleDirections = 0;
	// Implement PI's choices (if still possible)
	// int numPossibleDirectionsPi = 0;
	
	for (int i = 0; i < NUM_DIRECTIONS * 2; i++) {
		// LINE PANIC! If a line is detected on the left, only a right turn is allowed between 90deg and 180deg
		if (data.IR_left_detected && ((i < ((float)NUM_DIRECTIONS / 4.0) + NUM_DIRECTIONS) || (i > ((float)NUM_DIRECTIONS / 4.0) * 2.0 + NUM_DIRECTIONS)) ) {
			directionArray[i] = 0;
		}
		// LINE PANIC! If a line is detected on the right, only a left turn is allowed between 90deg and 180deg
		if (data.IR_right_detected && ( (i < ((float)NUM_DIRECTIONS / 4.0)) || (i > ((float)NUM_DIRECTIONS / 4.0)*2.0) )) {
			directionArray[i] = 0;
		}
		// ULTRASOUND PANIC! Check the obstacle_turret_distances array to see if there are unallowed directions
		if ((i < (float)NUM_DIRECTIONS / 4.0) || ((i > (((float)NUM_DIRECTIONS / 4.0)*3.0)) && (i < (((float)NUM_DIRECTIONS / 4.0)*5.0))) || (i > (((float)NUM_DIRECTIONS / 4.0)*7.0))) {
			// Incremental value for turret directions per robot direction
			float kinc = ((((float)NUM_TURRET_DIRECTIONS - 1) * 3.0) / (float)NUM_DIRECTIONS);
			// left most turret direction for this robot direction
			int kinit = ((int)((float)(i % NUM_DIRECTIONS) + (float)NUM_DIRECTIONS / 6.0) % NUM_DIRECTIONS) * kinc - kinc;
			// for all turret directions relative to this robot direction, check obstacle sensor data
			for (int k = kinit; k <= (kinit + 2*kinc); k++) {
				if (k < 0) k = 0;
				if (data.obstacle_turret_distances[k] > OBSTACLE_TOO_CLOSE) {
					directionArray[i] = 0;
				}
			}
		}
		
		DEBUG_PRINT(" ");
		DEBUG_PRINT(directionArray[i]);
		if (directionArray[i]) {
			numPossibleDirections++;
			// Implement PI's choices (if still possible)
			// if(PIarray[i]) {numPossibleDirectionsPi++;}
		}
	}

	// Implement PI's choices (if still possible)
	// if (numPossibleDirectionsPi) {
	//for (int i = 0; i < NUM_DIRECTIONS * 2; i++) {
	// if(!PIarray[i]) {directionArray[i] = 0}
	// }
	// }
	
	return numPossibleDirections;
}

void checkSample() {
	if ((dataToPiChangedBits & (CHANGED_SENS_SAMPLETURRET | CHANGED_SENS_SAMPLEGRIPPER)) && (data.sample_turret_detected || data.sample_gripper_detected)) {
		// One of the turret sees a sample (or the lab)
		opMode(OPMODE_CHECKSAMPLE);
		stopAllServos();
		sensSampleTurret();
		sensSampleGripper();
	}
}

bool confirmSample() {
	if (data.sample_turret_detected || data.sample_gripper_detected) {
		if (!data.sample_gripper_detected) {
			// Turn robot to sample
			// If still no sample_gripper_detected return false
		}
		// What is distance
	}
}

void initialSequence() {
	do {
		moveStraight(0, CAL_INITIAL_DISTANCE1);
	} while(timerMovementStop);
	// Set x and y value of robot to (0, 0) for lab/starting location.
	data.robot_curr_x = 0;
	data.robot_curr_y = 0;
	do {
		moveStraight(0, CAL_INITIAL_DISTANCE2);
	}	while(timerMovementStop);
}

void calculateOrientation() {
	// current + compass
}

// ***********************
// ** ARDUINO FUNCTIONS **
// ***********************

void setup() {
	// initial communication between Arduino and RPi
	// Send some values:
	// - Number of possible directions
	
	Wire.begin();
#ifdef DEBUG
	Serial.begin(9600);
#endif
	
	DEBUG_PRINT("\n\n\n** ROVER ");
	DEBUG_PRINT(ROVER);
	DEBUG_PRINT(" **\n\n");
	
	initTurretSequence(turretSequence, NUM_TURRET_DIRECTIONS);
	for(int i = 0; i < NUM_TURRET_DIRECTIONS; i++) {
		DEBUG_PRINT(turretSequence[i]);
		DEBUG_PRINT(" ");
	}
	DEBUG_PRINT("\n\n");
	
	// initialise compass communication
	compass = HMC5883L();
	// Gauss scale 1.3
	int error = compass.SetScale(1.3);
	if(error != 0){
		DEBUG_PRINT(compass.GetErrorText(error));
		DEBUG_PRINT("\n");
	}
	error = compass.SetMeasurementMode(Measurement_Continuous);
	if(error != 0){
		DEBUG_PRINT(compass.GetErrorText(error));
	}
	
	servoLeft.attach(PIN_SERVO_LEFT);
	servoRight.attach(PIN_SERVO_RIGHT);
	servoTurret.attach(PIN_SERVO_TURRET);
	servoGripper.attach(PIN_SERVO_GRIPPER);
	
	turnTurretTo(0);
	
	delay(100); // Make sure all actuators are at their starting position
}

void loop() {
	
	//sensCompass();
	sensObstacleTurret();
	sensIRLine(L);
	sensIRLine(R);
	//sensSampleTurret();
	//sensSampleGripper();
	
	//sensLab();
	
	//checkSample();
	checkFreePath();

	// calculateOrientation();
	// calculateLocationOffset();
	
	if (checkRPiActive()) {
		operationModeDefault = (operationModeDefault == OPMODE_INITIALSEQUENCE ? OPMODE_INITIALSEQUENCE : OPMODE_WAIT);
		//sendData(PI_DATATYPE_ALL, 'lala', 'lala');
		//readData();
	} else {
		// STANDALONE ALGORITHM
		operationModeDefault = (operationModeDefault == OPMODE_INITIALSEQUENCE ? OPMODE_INITIALSEQUENCE : OPMODE_MAPPING);
	}
	dataToPiChangedBits = 0; // Clear changed bits
	
	
	switch(operationMode) {
		case OPMODE_WAIT:
		counterMovement = 0;
		timerMovementStart = 0;
		timerMovementStop = 0;
		opMode(operationModeDefault);
		turnTurretToNext();
		break;
		
		case OPMODE_INITIALSEQUENCE:
		initialSequence();
		operationModeDefault = OPMODE_WAIT;
		break;
		
		case OPMODE_RANDOM:
		break;
		
		case OPMODE_MAPPING:
		turnTurretToNext();
		checkBestRoute(0);
		moveTurnTo(currValDirection);
		moveStraight(currValDirection, 30.0);
		break;
		
		case OPMODE_CHECKSAMPLE:
		break;
		
		case OPMODE_GRABSAMPLE:	
		break;
		
		case OPMODE_GOTOLABLOCATION:
		sensBeaconTurret();
		break;
		
		case OPMODE_WAITFORLAB:
		break;
		
		case OPMODE_FINDMAGNET:
		break;
		
		case OPMODE_LABSEQUENCE:
		break;
		
		default:
		break;
	}
	
	// Delay for stability.
	delay(2);
}