/* motor.c
 *
 * This file contains all motor related controls and functionality. Please refer to
 * the inline comments below for a detailed description of how each routine operates.
 */

#include "motor.h"

// For the base drive motors, we only will need to control the direction signals,
// since the speed is controlled via PWM.
#define kMotorMask ((1 << kLeftBaseMotorDirection) | (1 << kRightBaseMotorDirection))
	
/*************************************************************************
 * ReverseRobot
 *
 * This code toggles the directional bits on the motors to run the robot
 * in reverse. It also initializes a timer which expires when the specified
 * reversal duration has elapsed.
 *************************************************************************/
void ReverseRobot(unsigned int duration) {
	 unsigned char command = ReadPORTG();
	 // Run the robot in reverse
	 command &= ~kMotorMask;
	 WritePORTG(command);
	 PWM_SetSpeed(kBaseReverseSpeed, kRightLeftSpeedChan);
	 TMR_InitTimer(kGeneralTimer, duration);
}

/*************************************************************************
 * RotateRobot
 *
 * To rotate the robot in a particular direction, one of the directional
 * bits needs to be toggled whilst the other must remain unchanged. Note
 * the case in which the direction is set as "none" is a special case which
 * causes the rotation to halt by disabling both motors.
 *************************************************************************/
void RotateRobot(Direction toTurn)
{
 unsigned char command = ReadPORTG();
 
 // Clear out the old values
 command &= (~kMotorMask);
 if (toTurn == kDirectionRight) {
 	dprintf("Motors turning right...\n");
	command |= (1 << kLeftBaseMotorDirection);
 } else if (toTurn == kDirectionLeft) {
   dprintf("Motors turning left...\n");
   command |= (1 << kRightBaseMotorDirection); 
 } else {
   PWM_SetSpeed(kDisableMotor, kRightLeftSpeedChan);
   return;					   // Disable motors 
 }
 PWM_SetSpeed(kBaseRotateSpeed, kRightLeftSpeedChan);
 WritePORTG(command);		 // Send the command
}

/*************************************************************************
 * RotateTurret
 *
 * This code provides the logic to manage the turret rotation. Note the
 * trapdoor for halting the turret motor (when the last and original position
 * are the same). Otherwise, the code needs to be concerned with the turret
 * direction and the right/left solenoid enables. The behavior can be described
 * as follows:
 *
 * Left->Center
 * ------------
 *	Raise right solenoid, set pitching speed for center, rotate right
 * Left->Right
 * -----------
 *	Double rotation time, set pitching speed for angled shooting, rotate right
 * Right->Center
 * -------------
 *	Raise left solenoid, set pitching speed for center, rotate left
 * Right->Left
 * -----------
 * 	Double rotation time, set pitching speed for angled shooting, rotate left
 * Center->Left
 * ------------
 *	Back off the left solenoid, drop left solenoid, rotate left;
 *	set angled pitching speed, rotate left
 * Center->Right
 * -------------
 *	Back off the right solenoid, drop right solenoid, rotate right;
 *	set angled pitching speed, rotate right
 *************************************************************************/
void RotateTurret(BeaconCode newPos, BeaconCode oldPos)
{
 unsigned int baseRotateTime = kBaseRotateTime;
 unsigned char command = ReadPORTG(), baseCommand;
 unsigned int pitchingSpeed = kPitchingRunCenter;
 unsigned int i,j;
 
 if (oldPos == newPos) {
 	// Shut off the motor; solenoids, etc. can stay as is
 	command &= (~(1 << kBaseTurretEnable) & 0xFF);
	WritePORTG(command);
	return;
 } 
 // Clear out the old value
 baseCommand = (1 << kBaseTurretDirection) |
 			   (1 << kLeftSolenoid) |
			   (1 << kRightSolenoid);
 command &= (~baseCommand & 0xFF);
 // Make sure the turret motor is enabled
 command |= (1 << kBaseTurretEnable);
 switch (oldPos) {
 		case kCenterBeacon:
			 switch (newPos) {
                                        // Note we do not need to back off the solenoid
                                        // if we are returning to x having just rotated
                                        // from x->center. We could detect this and avoid
                                        // it but this did not lead to any sort of mechanical
                                        // problem, whereas the sequence x->center, center->y,
                                        // x != y, would cause the turret to get caught on the
                                        // solenoid on y's side (blocking the turret). Thus,
                                        // we inserted the backoff stage into the code.
			 		case kRightBeacon:
						 dprintf("Going right!\n");
						 pitchingSpeed = kPitchingAngled;
						 WritePORTG(command | (1 << kBaseTurretDirection));
						 PWM_SetSpeed(pitchingSpeed, kPitchingSpeedChan);
						 BlockingCode(kTurretCorrectionTime);
						 break;
					case kLeftBeacon:
						 // Change direction (go left)
						 dprintf("Going left!\n");
						 pitchingSpeed = kPitchingAngled;
						 WritePORTG(command);
						 command |= (1 << kBaseTurretDirection);
						 PWM_SetSpeed(pitchingSpeed, kPitchingSpeedChan);
						 BlockingCode(kTurretCorrectionTime);
						 break;
					default:
						dprintf("Unexpected direction from center!\n");
						return;
						break;
			}
			 break;
		case kRightBeacon:
			 switch (newPos) {
			 		case kCenterBeacon:
						 dprintf("Going to center!\n");
						 // Raise left solenoid, turn left
						 command |= ((1 << kBaseTurretDirection) |
						 		 	(1 << kLeftSolenoid));
						 break;
					case kLeftBeacon:
						 dprintf("Going left!\n");
						 pitchingSpeed = kPitchingAngled;
						 // Double turn time
						 baseRotateTime *= 2;
						 command |= (1 << kBaseTurretDirection);
						 break;
					default:
						dprintf("Unexpected direction from right!\n");
						return;
						break;
			 }
			 break;
 		case kLeftBeacon:
			 switch (newPos) {
			 		case kCenterBeacon:
						 dprintf("Going to center!\n");
						 // Raise right solenoid, turn right
						 command |= (1 << kRightSolenoid);
						 break;
					case kRightBeacon:
						 // Double turn time
						 dprintf("Going right!\n");
						 pitchingSpeed = kPitchingAngled;
						 baseRotateTime *= 2;
						 break;
					default:
						dprintf("Unexpected direction from left!\n");
						break;
			}			
			 break;
		default:
			dprintf("Unknown initial position!\n");
			return;
			break;
 }
 // Set pitching speed
 PWM_SetSpeed(pitchingSpeed, kPitchingSpeedChan);
 WritePORTG(command);
 // Run motor and (re)set timer
 TMR_StopTimer(kGeneralTimer);
 TMR_InitTimer(kGeneralTimer, baseRotateTime);
}

/*************************************************************************
 * BlockingCode
 *
 * This is a small piece of code which delays event processing by occupying
 * the CPU for the specified duration. We use this code to overcome mechanical
 * delays present in the robot.
 *************************************************************************/
 void BlockingCode(int duration) {
	 // Delays execution for stated duration
	 TMR_StopTimer(kGeneralTimer);
 	 TMR_InitTimer(kGeneralTimer, duration);
	 while (!TMR_IsTimerExpired(kGeneralTimer)) { /* Block */ }
	 TMR_ClearTimerExpired(kGeneralTimer);
}