/*===================================================================
	ai_motion.c - Motion Control Functions
	Nels Pearson 22mar06

	This is the AI of the autonomous mode of operation.

 ====================================================================*/
// INCLUDEs
#include "ifi_aliases.h"
#include "ifi_default.h"
#include "user_routines.h"
#include "ai_motion.h"

// DEFINEs
enum AI_STATES { IDLE, MOVE_FWD, TURN_R, WAITING };


/**********************************************************************
	MoveControl - Controls direction and speed

	This function directly controls pwm01(R) and pwm02(L) motors.
	It is called every 18.5 ms if in AUTO mode.

	Input: struct SENSOR_DATA sd - current sensor data (ai_motion.h)

**********************************************************************/
void MoveControl(SENSOR_DATA* sd)
{
	static int waitCount = 0;
	static enum AI_STATES aiState = IDLE;
	static enum AI_STATES nextState = IDLE;
	static unsigned char currentSpeed = MOTOR_STOP;
	char newPwm01;
	char newPwm02;

	switch(aiState) {
		case IDLE:
			aiState = MOVE_FWD;
			break;

		case MOVE_FWD:
			if(sd->SonarC > FAR_OBJ) {
				if(currentSpeed < TOP_SPEED) {
					++currentSpeed;
				}
			} else if(sd->SonarC > ADV_OBJ) {
				if(currentSpeed < MID_SPEED) {
					++currentSpeed;
				} else if(currentSpeed > MID_SPEED) {
					--currentSpeed;
				}
			} else if(sd->SonarC > NEAR_OBJ) {
				if(currentSpeed < LOW_SPEED) {
					++currentSpeed;
				} else if(currentSpeed > LOW_SPEED) {
					--currentSpeed;
				}
			} else {
				// Obstical. Turn away.
				currentSpeed = MOTOR_STOP;
				aiState = TURN_R;
				waitCount = 0;
			}
			newPwm01 = newPwm02 = currentSpeed;
			break;

		// Turn away until Sonar is > 100, then keep turning for the same amount of time.
		case TURN_R:
			++waitCount;
			currentSpeed = MID_SPEED;
			if(sd->SonarC > 100) {
				aiState = MOVE_FWD;
				currentSpeed = MOTOR_STOP;
				newPwm01 = newPwm02 = currentSpeed;
			}
			// A little inefficient to keep updating. May remove and use 2 Wait conditions.
			newPwm01 = currentSpeed;
			newPwm02 = (~currentSpeed) + 1;
			break;

		default:
			aiState = 	IDLE;
			currentSpeed = MOTOR_STOP;
			newPwm01 = newPwm02 = currentSpeed;
			break;
	}
	// Update motor contol.
	pwm01 = newPwm01;
	pwm02 = (~newPwm02) + 1;
}