/**************************************************************************

Drive Motor Module

Left Drive Motor: U0 (PWM), T7 (Dir)
Right Drive Motor: U1 (PWM), E7 (Dir)

**************************************************************************/

/*---------------------------- Include Files ----------------------------*/
#include <stdio.h>
#include <timers12.h>
#include "PWM.h"
#include "ADS12.h"
#include "game.h"

#include "drive_motor.h"

/*--------------------------- Module Defines ----------------------------*/
// Define Motor Direction Pin
#define	LEFT_WHEEL_FORWARD		PTT |= BIT7HI
#define	LEFT_WHEEL_REVERSE		PTT &= BIT7LO
#define 	RIGHT_WHEEL_FORWARD		PORTE |= BIT7HI
#define	RIGHT_WHEEL_REVERSE		PORTE &= BIT7LO

#define	TAPE_THRESHOLD		900

/*---------------------------- Module Types -----------------------------*/
/*-------------------------- Module Variables ---------------------------*/
/*-------------------------- Module Functions ---------------------------*/
void MotorStop(unsigned char wheel);
/*----------------------------- Module Code -----------------------------*/

// InitializeDriveMotors: Takes Nothing, Returns nothing and initializes Motors subsystem
void InitializeDriveMotors(void) {

	PWMOnOff(1,1,1,1,1); // Turn PWM Channel 0, 1 on (U0 and U1)
  	PWMFrequency(0,800); // group 0
	MotorStop(LEFT_DRIVE);
	MotorStop(RIGHT_DRIVE);
	DDRE |= BIT7HI; // Right Dir
	DDRT |= BIT7HI; // Left Dir
	(void)ADS12_Init("IIIIAAOO"); // Bump sensors + tape sensors
}

// MotorStop: Takes LEFT or RIGHT for the wheel and stops it
void MotorStop(unsigned char wheel){

	switch (wheel){
		case LEFT: PWMDutyCycle(LEFT_DRIVE, 0); break;

		case RIGHT:	PWMDutyCycle(RIGHT_DRIVE, 0); break;

		default:
		break;
	}
}

// MotorReverse: Takes LEFT or RIGHT for the wheel and drives it backward at the specified speed
void MotorReverse(unsigned char wheel, unsigned char speed){

	switch (wheel){
		case LEFT:
			LEFT_WHEEL_REVERSE;
			PWMDutyCycle(LEFT_DRIVE, speed);
		break;

		case RIGHT:
			RIGHT_WHEEL_REVERSE;
			PWMDutyCycle(RIGHT_DRIVE, speed);
		break;

		default:
		break;
	}
}

// MotorForward: takes LEFT or RIGHT for the wheel and drives it forward at the specified speed
void MotorForward(unsigned char wheel, unsigned char speed){

	switch (wheel){
		case LEFT:
			LEFT_WHEEL_FORWARD;
			PWMDutyCycle(LEFT_DRIVE, speed);
		break;

		case RIGHT:
			RIGHT_WHEEL_FORWARD;
			PWMDutyCycle(RIGHT_DRIVE, speed);
		break;

		default:
		break;
	}
}

void RotateCW(void){

	MotorForward(LEFT_DRIVE, 34);
	MotorReverse(RIGHT_DRIVE, 34);
}

void RotateCCW(void){

	MotorReverse(LEFT_DRIVE, 34);
	MotorForward(RIGHT_DRIVE, 34);
}

void StopDriveMotors(void) {

	MotorStop(LEFT_DRIVE);
	MotorStop(RIGHT_DRIVE);
}

// CheckBumpSensor: takes NORTH, EAST, WEST, SOUTH bumpSensor and returns whether it is bumped
unsigned char CheckBumpSensor(unsigned char bumpSensor) {

	switch(bumpSensor) {
		case NORTH:
			if((PTIAD & BIT7HI) == BIT7HI)
				return 1;
			else
				return 0;
		break;

		case EAST:
			if((PTIAD & BIT6HI) == BIT6HI)
				return 1;
			else
				return 0;
		break;

		case SOUTH:
			if((PTIAD & BIT5HI) == BIT5HI)
				return 1;
			else
				return 0;
		break;

		case WEST:
			if((PTIAD & BIT4HI) == BIT4HI)
				return 1;
			else
				return 0;
		break;

		default:
		break;
	}
}

// CheckTapeSensor: takes FRONT or MIDDLE and returns whether it is activated
unsigned char CheckTapeSensor(unsigned char tapeSensor) {

	switch(tapeSensor) {

	  // unsigned char
		case FRONT:
			if (ADS12_ReadADPin(3) > TAPE_THRESHOLD)
				return 1;
			else
				return 0;
			break;
		case MIDDLE:
			if (ADS12_ReadADPin(2) > TAPE_THRESHOLD)
				return 1;
			else
				return 0;
			break;

	}
	/*
	// unsigned int
	case FRONT: return(ADS12_ReadADPin(3)); break;
	case MIDDLE: return(ADS12_ReadADPin(2)); break;

	}
	*/


}